MOBILE ROBOTICS [second edition] 9781786305237, 1786305232

Citation preview

Mobile Robotics

Revised and Updated 2nd Edition Series Editor Hisham Abou-Kandil

Mobile Robotics

Luc Jaulin

First edition published 2015 in Great Britain and the United States by ISTE Press and Elsevier © ISTE Press 2015. First published 2019 in Great Britain and the United States by ISTE Ltd and John Wiley & Sons, Inc.

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 and licenses issued by the CLA. Enquiries concerning reproduction outside these terms should be sent to the publishers at the undermentioned address: ISTE Ltd 27-37 St George’s Road London SW19 4EU UK

John Wiley & Sons, Inc. 111 River Street Hoboken, NJ 07030 USA

www.iste.co.uk

www.wiley.com

© ISTE Ltd 2019 The rights of Luc Jaulin to be identified as the author of this work have been asserted by him in accordance with the Copyright, Designs and Patents Act 1988. Library of Congress Control Number: 2019943737 British Library Cataloguing-in-Publication Data A CIP record for this book is available from the British Library ISBN 978-1-78630-523-7

Contents

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

ix

Chapter 1. Three-dimensional Modeling . . . . . . . . . . . . . . . . . .

1

1.1. Rotation matrices . . . . . . . . . . . . . . . 1.1.1. Definition . . . . . . . . . . . . . . . . . . 1.1.2. Lie group . . . . . . . . . . . . . . . . . . 1.1.3. Lie algebra . . . . . . . . . . . . . . . . . 1.1.4. Rotation vector . . . . . . . . . . . . . . . 1.1.5. Adjoint . . . . . . . . . . . . . . . . . . . 1.1.6. Rodrigues rotation formulas . . . . . . . 1.1.7. Coordinate system change . . . . . . . . 1.2. Euler angles . . . . . . . . . . . . . . . . . . 1.2.1. Definition . . . . . . . . . . . . . . . . . . 1.2.2. Rotation vector of a moving Euler matrix 1.3. Inertial unit . . . . . . . . . . . . . . . . . . . 1.4. Dynamic modeling . . . . . . . . . . . . . . 1.4.1. Principle . . . . . . . . . . . . . . . . . . 1.4.2. Modeling a quadrotor . . . . . . . . . . . 1.5. Exercises . . . . . . . . . . . . . . . . . . . . 1.6. Corrections . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

1 2 3 4 5 6 7 8 11 11 13 14 17 17 18 20 37

Chapter 2. Feedback Linearization . . . . . . . . . . . . . . . . . . . . . .

65

2.1. Controlling an integrator chain . . . . . . . . . 2.1.1. Proportional-derivative controller . . . . . 2.1.2. Proportional-integral-derivative controller 2.2. Introductory example . . . . . . . . . . . . . . 2.3. Principle of the method . . . . . . . . . . . . . 2.3.1. Principle . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . . . . . . . . . . . . .

. . . . . .

. . . . . .

65 66 67 68 69 69

vi

Mobile Robotics

2.3.2. Relative degree . . . . . . . . . . . . . . 2.3.3. Differential delay matrix . . . . . . . . 2.3.4. Singularities . . . . . . . . . . . . . . . 2.4. Cart . . . . . . . . . . . . . . . . . . . . . . 2.4.1. First model . . . . . . . . . . . . . . . 2.4.2. Second model . . . . . . . . . . . . . . 2.5. Controlling a tricycle . . . . . . . . . . . . 2.5.1. Speed and heading control . . . . . . . 2.5.2. Position control . . . . . . . . . . . . . 2.5.3. Choosing another output . . . . . . . . 2.6. Sailboat . . . . . . . . . . . . . . . . . . . . 2.6.1. Polar curve . . . . . . . . . . . . . . . . 2.6.2. Differential delay . . . . . . . . . . . . 2.6.3. The method of feedback linearization . 2.6.4. Polar curve control . . . . . . . . . . . 2.7. Sliding mode . . . . . . . . . . . . . . . . . 2.8. Kinematic model and dynamic model . . . 2.8.1. Principle . . . . . . . . . . . . . . . . . 2.8.2. Example of the inverted rod pendulum 2.8.3. Servo-motors . . . . . . . . . . . . . . . 2.9. Exercises . . . . . . . . . . . . . . . . . . . 2.10. Corrections . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . .

. 71 . 72 . 73 . 75 . 75 . 76 . 78 . 78 . 80 . 81 . 82 . 83 . 83 . 84 . 87 . 87 . 90 . 90 . 91 . 94 . 95 . 107

Chapter 3. Model-free Control . . . . . . . . . . . . . . . . . . . . . . . . . 133 3.1. Model-free control of a robot cart . . . . . . . 3.1.1. Proportional heading and speed controller 3.1.2. Proportional-derivative heading controller 3.2. Skate car . . . . . . . . . . . . . . . . . . . . . 3.2.1. Model . . . . . . . . . . . . . . . . . . . . . 3.2.2. Sinusoidal control . . . . . . . . . . . . . . 3.2.3. Maximum thrust control . . . . . . . . . . 3.2.4. Simplification of the fast dynamics . . . . 3.3. Sailboat . . . . . . . . . . . . . . . . . . . . . 3.3.1. Problem . . . . . . . . . . . . . . . . . . . . 3.3.2. Controller . . . . . . . . . . . . . . . . . . . 3.3.3. Navigation . . . . . . . . . . . . . . . . . . 3.3.4. Experiment . . . . . . . . . . . . . . . . . . 3.4. Exercises . . . . . . . . . . . . . . . . . . . . . 3.5. Corrections . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

. . . . . . . . . . . . . . .

134 134 136 137 138 140 140 142 145 145 146 152 153 155 168

Contents

vii

Chapter 4. Guidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183 4.1. Guidance on a sphere . . . . . . 4.2. Path planning . . . . . . . . . . . 4.2.1. Simple example . . . . . . . 4.2.2. Bézier polynomials . . . . . 4.3. Voronoi diagram . . . . . . . . . 4.4. Artificial potential field method 4.5. Exercises . . . . . . . . . . . . . 4.6. Corrections . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

183 187 187 188 189 191 192 201

Chapter 5. Instantaneous Localization . . . . . . . . . . . . . . . . . . . 221 5.1. Sensors . . . . . . . . . . . . . . . . . . 5.2. Goniometric localization . . . . . . . . 5.2.1. Formulation of the problem . . . . . 5.2.2. Inscribed angles . . . . . . . . . . . 5.2.3. Static triangulation of a plane robot 5.2.4. Dynamic triangulation . . . . . . . 5.3. Multilateration . . . . . . . . . . . . . . 5.4. Exercises . . . . . . . . . . . . . . . . . 5.5. Corrections . . . . . . . . . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

221 225 225 226 228 229 230 231 236

Chapter 6. Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 243 6.1. Quadratic functions . . . . . . . . . . . 6.1.1. Definition . . . . . . . . . . . . . . . 6.1.2. Derivative of a quadratic form . . . 6.1.3. Eigenvalues of a quadratic function 6.1.4. Minimizing a quadratic function . . 6.2. The least squares method . . . . . . . . 6.2.1. Linear case . . . . . . . . . . . . . . 6.2.2. Nonlinear case . . . . . . . . . . . . 6.3. Exercises . . . . . . . . . . . . . . . . . 6.4. Corrections . . . . . . . . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

243 243 244 245 245 246 246 248 250 253

Chapter 7. Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263 7.1. Covariance matrices . . . . . . . . . . . . 7.1.1. Definitions and interpretations . . . . 7.1.2. Properties . . . . . . . . . . . . . . . . 7.1.3. Confidence ellipse . . . . . . . . . . . 7.1.4. Generating Gaussian random vectors 7.2. Unbiased orthogonal estimator . . . . . . 7.3. Application to linear estimation . . . . . 7.4. Kalman filter . . . . . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

263 263 266 267 268 269 274 275

viii

Mobile Robotics

7.5. Kalman–Bucy . . . . . 7.6. Extended Kalman filter 7.7. Exercises . . . . . . . . 7.8. Corrections . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

279 282 283 298

Chapter 8. Bayes Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 329 8.1. Introduction . . . . . . . . . . . . . . . 8.2. Basic notions of probabilities . . . . . . 8.3. Bayes filter . . . . . . . . . . . . . . . . 8.4. Bayes smoother . . . . . . . . . . . . . 8.5. Kalman smoother . . . . . . . . . . . . 8.5.1. Equations of the Kalman smoother 8.5.2. Implementation . . . . . . . . . . . 8.6. Exercises . . . . . . . . . . . . . . . . . 8.7. Corrections . . . . . . . . . . . . . . . . References Index

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

329 329 332 334 335 335 336 337 345

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 359

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361

Introduction

A mobile robot can be defined as a mechanical system capable of moving in its environment in an autonomous manner. For that purpose, it must be equipped with: – sensors, which will collect knowledge of its surroundings (which it is more or less aware of) and determine its location; – actuators, which will allow it to move; – an intelligence (or algorithm, regulator), which will allow it to compute, based on the data gathered by the sensors, the commands to send to the actuators in order to perform a given task. Finally, to this we must add the surroundings of the robot, which correspond to the world in which it evolves, and its mission, which is the task it has to accomplish. Mobile robots are constantly evolving, mainly from the beginning of the 2000s, in military domains (airborne drones [BEA 12], underwater robots [CRE 14], etc.), and even in medical and agricultural fields. They are in particularly high demand for performing tasks considered to be painful or dangerous to humans. This is the case for instance in mine-clearing operations, the search for black boxes of damaged aircraft on the seabed and planetary exploration. Artificial satellites, launchers (such as Ariane V), driverless subways and elevators are examples of mobile robots. Airliners, trains and cars evolve in a continuous fashion towards more and more autonomous systems and will very probably become mobile robots in the following decades. Mobile robotics is the discipline which looks at the design of mobile robots [LAU 01]. It is based on other disciplines such as automatic control, signal processing, mechanics, computing and electronics. The aim of this book is to give an overview of the tools and methods of robotics which will aid the design of mobile robots. The robots will be modeled by state equations, i.e. first-order (mostly nonlinear) differential equations. These state equations can be obtained by using the

x

Mobile Robotics

laws of mechanics. It is not in our objectives to teach, in detail, the methods of robot modeling (refer to [JAU 05] and [JAU 15] for more information on the subject), but merely to recall its principles. By modeling, we mean obtaining the state equations. This step is essential for simulating robots as well as designing controllers. We will however illustrate the principle of modeling in Chapter 1 on deliberately three-dimensional examples. This choice was made in order to introduce important concepts in robotics such as Euler angles and rotation matrices. For instance, we will be looking at the dynamics of a wheel and the kinematics of an underwater robot. Mobile robots are strongly nonlinear systems and only a nonlinear approach allows the construction of efficient controllers. This construction is the subject of Chapters 2 and 3. Chapter 2 is mainly based on control methods that rely on the utilization of the robot model. This approach will make use of the concept of feedback linearization, which will be introduced and illustrated through numerous examples. Chapter 3 presents more pragmatic methods which do not use the state model of the robot and which will be referred to as without model or mimetic. This approach uses a more intuitive representation of the robot and is adapted to situations in which the robots are relatively simple to control remotely, such as in the case of cars, sailing boats or airplanes. Chapter 4 looks at guidance, which is placed at a higher level than control. In other words, it focuses on guiding and supervising the system which is already under control by the tools presented in Chapters 2 and 3. There will therefore be an emphasis on finding the instruction to give to the controller in order for the robot to accomplish its given task. The guidance will then have to take into account knowledge of the surroundings, the presence of obstacles and the roundness of the Earth. The nonlinear control and guidance methods require good knowledge of the state variables of the system, such as those which define the position of the robot. These position variables are the most difficult to find and Chapter 5 focuses on the problem of positioning. It introduces the classical nonlinear approaches that have been used for a very long time by humans for positioning, such as observing beacons and stars, using a compass or counting steps. Although positioning can be viewed as a particular case of state observation, the specific methods derived from it warrant a separate chapter. Chapter 6 on identification focuses on estimating, with a certain precision, non-measured quantities (parameters, position) from other, measured ones. In order to perform this identification, we will mainly be looking at the so-called least squares approach, which consists of finding the vector of variables that minimizes the sum of the squares of the errors. Chapter 7 presents the Kalman filter. This filter can be seen as a state observer for dynamic linear systems with coefficients that vary in time. A generalization of the Kalman filter to the case where the functions are nonlinear and the noise is non-Gaussian is provided in Chapter 8. The resulting observer, which is called the Bayes filter, computes the probability density function of the state vector at specific times. The M ATLAB® and P YTHON code related to some the exercises of this book, together with explanatory videos, can be found at the following address: www.ensta-bretagne.fr/jaulin/isterob.html

1 Three-dimensional Modeling

This chapter looks at the three-dimensional modeling of a solid (non-articulated) robot. Such modeling is used to represent airplanes, quadcopters, submarines, and so on. Through this modeling we will introduce a number of fundamental concepts in robotics such as state representation, rotation matrices and Euler angles. The robots, whether mobile, manipulator or articulated, can generally be put into a state representation form: !

x(t) ˙ = f (x(t), u(t)) y(t) = g(x(t), u(t))

where x is the state vector, u the input vector and y the vector of measurements [JAU 05]. We will call modeling the step which consists of finding a more or less accurate state representation of the robot. In general, constant parameters may appear in the state equations (such as the mass and the moment of inertia of a body, viscosity, etc.). In such cases, an identification step might prove to be necessary. We will assume that all of the parameters are known. Of course, there is no systematic methodology that can be applied for modeling a mobile robot. The aim of this chapter is to present the tools which allow us to reach a state representation of three-dimensional solid robots in order for the reader to acquire a certain experience which will be helpful when modeling his/her own robots. This modeling will also allow us to recall a number of important concepts in Euclidean geometry, which are fundamental in mobile robotics. This chapter begins by recalling a number of important concepts in kinematics which will be useful for the modeling. 1.1. Rotation matrices For three-dimensional modeling, it is essential to have a good understanding of the concepts related to rotation matrices, which are recalled in this section. It is by using Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

2

Mobile Robotics

this tool that we will perform our coordinate system transformations and position our objects in space. 1.1.1. Definition Let us recall that the j th column of the matrix of a linear application of Rn → R represents the image of the j th vector ej of the standard basis (see Figure 1.1). Thus, the expression of a rotation matrix of angle θ in the plane R2 is given by: n

R=

"

cos θ − sin θ sin θ cos θ

#

.

Figure 1.1. Rotation of angle θ in a plane

Concerning rotations in the space R3 (see Figure 1.2), it is important to specify the axis of rotation. We distinguish three main rotations: the rotation around the Ox axis, around the Oy axis and around the Oz axis.

Figure 1.2. Rotations in R3 following various viewing angles

Three-dimensional Modeling

3

The associated matrices are respectively given by:     1 0 0 cos θy 0 sin θy 0 1 0 , Rx =  0 cos θx − sin θx  , Ry =  0 sin θx cos θx − sin θy 0 cos θy   cos θz − sin θz 0 Rz =  sin θz cos θz 0  . 0 0 1

Let us recall the formal definition of a rotation. A rotation is a linear application which is an isometry (i.e. it preserves the scalar product) and is direct (it does not change the orientation in space). T HEOREM.– A matrix R is a rotation matrix if and only if: RT · R = I and det R = 1. P ROOF.– The scalar product is preserved by R if, for any u and v in Rn , we have: T

(Ru) · (Rv) = uT RT Rv = uT v. Therefore RT R = I. The symmetries relative to a plane, as well as all the other improper isometries (isometries that change the orientation of space, such as a mirror), also verify the property RT ·R = I. The condition det R = 1 allows us to be limited to the isometries which are direct. ! 1.1.2. Lie group The set of rotation matrices of Rn forms a group with respect to the multiplication. It is referred to as a special orthogonal group (special because det R = 1, orthogonal because RT ·R = I) and denoted by SO(n). It is trivial to check that (SO(n), ·) is a group where I is the neutral element. Moreover, the multiplication and the inversion are both smooth. This makes SO(n) a Lie group which is a manifold of the set of matrices Rn×n . The set Rn×n of n × n-matrices is of dimension n2 . Since the matrix RT · R is always symmetric, the matrix equation RT · R = I can be decomposed into n(n+1) 2 independent scalar equations. For instance, for n = 2, we have 2(2+1) = 3 scalar 2 equations:  2 " # " #T " #  a + b2 = 1 a b a b 1 0 · = ⇔ ac + bd = 0 c d c d 0 1  2 c + d2 = 1

4

Mobile Robotics

As a consequence, the set SO(n) forms a manifold of dimension d = n2 − n(n+1) . 2

– for n = 1, we get d = 0. The set SO(1) is a singleton which contains a single rotation matrix: R = 1; – for n = 2, we get d = 1. We need a unique parameter (or angle) to represent the rotations of SO(2); – for n = 3, we get d = 3. We need three parameters (or angles) to represent SO(3). 1.1.3. Lie algebra An algebra is an algebraic structure (A, +, ×, ·) over a body K, if (i) (A, +, ·) is a vector space over K; (ii) the multiplication rule × of A × A → A is left- and rightdistributive with respect to +; and (iii) for all α, β ∈ K, and for all x, y ∈ A, α·x×β·y and (αβ) · (x × y). Notice that in general, an algebra is non-commutative (x × y &= y × x) and non-associative ((x × y) × z &= x × (y × z)). A Lie algebra (G, +, [ ] , ·) is a non-commutative and non-associative algebra in which multiplication, denoted by a so-called Lie bracket, verifies that (i) [·, ·] is bilinear, i.e. linear with respect to each variable; (ii) [x, y] = −[y, x] (antisymmetry) and (iii) [x, [y, z]] + [y, [z, x]] + [z, [x, y]] = 0 (Jacobi relation). For Lie groups, we can define the associated Lie algebras. Lie algebras allow us to consider infinitesimal motions around a given element (i.e. a rotation matrix) in order to use derivatives or differential methods. Consider the rotation matrix I of SO (n) corresponding to the identity. If we move I by adding a small matrix, say A · dt of Rn×n , we generally do not obtain a rotation matrix. We are interested in matrices A such that I + A · dt ∈ SO (n). We have T

(I + A · dt) · (I + A · dt) = I, i.e. A·dt+AT ·dt = 0+o (dt). Therefore, A should be skew-symmetric. This means that we are able to move in SO (n) around I by adding infinitesimal skew-symmetric matrices A · dt that are not elements of SO(n). This corresponds to a new operation in SO (n) which is not the multiplication that we already had. Formally, we define the Lie algebra associated with SO (n) as follows: + , Lie (SO(n)) = A ∈ Rn×n | I + A · dt ∈ SO(n)

and it corresponds to the skew-symmetric matrices of Rn×n .

If we now want to move around any matrix R of SO (n), we generate a rotation matrix around I and we transport it to R. We get R (I + A · dt) where A is skew-symmetric. This means that we add to R the matrix R · A · dt.

Three-dimensional Modeling

5

In robotics, Lie groups are often used to describe transformations (such as translations or rotations). The Lie algebra corresponds to velocities or equivalently to infinitesimal transformations. Lie group theory is useful, but requires some non-trivial mathematical backgrounds that are beyond the scope of this book. We will try to focus on SO (3) or use more classical tools such as Euler angles and rotation vectors, which are probably less general but are sufficient for control purposes. 1.1.4. Rotation vector If R is a rotation matrix depending on time t, by differentiating the relation RRT = I, we get ˙ · RT + R · R ˙ T = 0. R ˙ · RT is a skew-symmetric matrix (i.e. it satisfies AT = −A Thus, the matrix R and therefore its diagonal contains only zeroes, and for each element of A, we have aij = −aji ). We may therefore write, in the case where R is of dimension 3 × 3:   0 −ωz ωy ˙ · R T =  ωz 0 −ωx  . R [1.1] −ωy ωx 0 - The.vector ω = (ωx , ωy , ωz ) is called the rotation vector associated with the pair ˙ . It must be noted that R ˙ is not a matrix with good properties (such as for R, R ˙ · RT has instance the fact of being skew-symmetric). On the other hand, the matrix R the structure of equation [1.1] since it allows positioning within the coordinate system in which the rotation is performed, and this is due to the change of basis performed by RT . We will define the cross product between two vectors ω and x ∈ R3 as follows:      x1 x 3 ω y − x2 ω z ωx ω ∧ x =  ω y  ∧  x 2  =  x1 ω z − x3 ω x  ωz x3 x2 ω x − x1 ω y    0 −ωz ωy x1 0 −ωx   x2  . =  ωz −ωy ωx 0 x3 

6

Mobile Robotics

1.1.5. Adjoint For each vector ω = (ωx , ωy , ωz ), we may adjoin the skew-symmetric matrix: 

0 Ad (ω) =  ωz −ωy

−ωz 0 ωx

 ωy −ωx  0

which can be interpreted as the matrix associated with a cross product by the vector ω. The matrix Ad (ω) is also written ω∧. P ROPOSITION.– If R(t) is a rotation matrix that depends on time, its rotation vector is given by: . ˙ · RT . ω = Ad−1 R [1.2] P ROOF.– This relation is a direct consequence of equation [1.1]. ! P ROPOSITION.– If R is a rotation matrix in R3 and if a is a vector of R3 , we have: Ad (R · a) = R · Ad (a) · RT

[1.3]

which can also be written as (R · a) ∧ = R · (a∧) · RT . P ROOF.– Let x be a vector of R3 . We have: / 0 Ad (R · a) · x = (R ·/a) ∧ x = (R · a) ∧ R · RT x 0 = R · a ∧ RT · x = R · Ad (a) · RT · x. !

P ROPOSITION.– (Duality) We have: ˙ = Ad(RT ω). RT R

[1.4]

˙ is associated with the rotation This relation expresses the fact that the matrix RT R vector ω, associated with R (t) but expressed in the coordinate system associated with ˙ · RT is associated with the same vector, but this time expressed in the R, whereas R coordinate system of the standard basis. P ROOF.– We have: . / 0 [1.3] ˙ = RT R ˙ · RT R [1.2] RT R = RT · Ad (ω) · R = Ad RT ω . !

Three-dimensional Modeling

7

1.1.6. Rodrigues rotation formulas Matrix exponential. Given a square matrix M of dimension n, its exponential can be defined as: eM = In + M +

∞

11 1 2 1 M + M3 + · · · = Mi 2! 3! i! i=0

where In is the identity matrix of dimension n. It is clear that eM is of the same dimension as M. Here are some of the important properties concerning the exponentials of matrices. If 0n is the zero matrix of n × n and if M and N are two matrices n × n, then: e0n = In M+N eM/.eN = (if the matrices commute) 0 e d Mt Mt e = Me dt

Matrix logarithm. Given a matrix M, the matrix L is said to be a matrix logarithm of M if eL = M. As for complex numbers, the exponential function is not a one-toone function and matrices may have more than one logarithm. Using power series, we define the logarithm of a square matrix as log M =

∞ i+1 1 (−1) i=1

i

i

(M − In ) .

The sum is convergent if M is close to identity. Rodrigues formulas. A rotation matrix R has an axis represented by a unit vector n and an angle α with respect to this axis. From n and α we can also generate the matrix R. The link R ↔ (n, α) is made by the following Rodrigues formulas (i) R = eαn∧ (ii) αn = log R

[1.5]

The first equation (i) will be shown in exercise 1.4 and (ii) is the reciprocal of (i). In these formulas, αn∧ is a notation to represent the matrix Ad (αn). As will be shown in exercise 1.4, R = eαn∧ is a rotation matrix and n is an eigenvector associated with the eigenvalue 1 of R. Axis and angle of a rotation matrix. Given a rotation matrix R, the axis can be represented by normalized eigenvector n associated with the eigenvalue λ = 1 (two of them can be found, but we need one). To compute α and n, we may use the relation

8

Mobile Robotics

αn = log R, which works well if R is close to identity. In the general case, it is more suitable to use the following proposition. P ROPOSITION.– Given a rotation matrix R, we have R = eαn∧ where α= n=

acos( tr(R)−1 /2 ) 0 · Ad−1 R − RT

1 2 sin α

[1.6]

P ROOF.– Take a rotation matrix R with eigenvalues 1, λ1 , λ2 and eigenvectors n, v1 , v2 . Consider the generalized polynomial f (x) = x − x−1 , where x is the indeterminate. From the correspondence theorem of eigenvalues/vectors, the eigenvalues of f (R) = R − R−1 = R − RT are f (1) = 0, f (λ2 ), f (λ3 ) and the eigenvectors are still n, v1 , v2 . Since f (R) is skew and f (R) · n = 0, / symmetric 0 we have f (R) ∝ Adn. Thus, the vector Ad−1 R − RT is an eigenvector of R associated with the eigenvalue 1. It provides us the axis of R. To find the angle α, we use the property that the trace of a matrix is similarity-invariant, which means that for any invertible matrix P, we have tr(R) = tr(P−1 · R · P). Take P as the rotation matrix with transforms R into a rotation along the first axis. We get   1 0 0 tr(R) = tr  0 cos α − sin α  = 1 + 2 cos α, 0 sin α cos α

which provides us with the angle of the rotation. Exercise 1.5 provides a geometrical proof of this proposition. !

1.1.7. Coordinate system change Let R0 : (o0 , i0 , j0 , k0 ) and R1 : (o1 , i1 , j1 , k1 ) be two coordinate systems and let u be a vector of R3 (refer to Figure 1.3). We have the following relation: u = x0 i0 + y0 j0 + z0 k0 = x1 i1 + y1 j1 + z1 k1 where (x0 , y0 , z0 ) and (x1 , y1 , z1 ) are the coordinates of u in R0 and R1 , respectively. Thus, for any vector v we have: *x0 i0 + y0 j0 + z0 k0 , v+ = *x1 i1 + y1 j1 + z1 k1 , v+ . By taking respectively v = i0 , j0 , k0 , we obtain the following three relations:   *x0 i0 + y0 j0 + z0 k0 , i0 + = *x1 i1 + y1 j1 + z1 k1 , i0 + *x0 i0 + y0 j0 + z0 k0 , j0 + = *x1 i1 + y1 j1 + z1 k1 , j0 +  *x0 i0 + y0 j0 + z0 k0 , k0 + = *x1 i1 + y1 j1 + z1 k1 , k0 +

Three-dimensional Modeling

9

Figure 1.3. Changing the coordinate system R0 to the system R1

However, since the basis (i0 , j0 , k0 ) of R0 is orthonormal, *i0 , i0 + = *j0 , j0 + = *k0 , k0 + = 1 and *i0 , j0 + = *j0 , k0 + = *i0 , k0 + = 0. Thus, these three relations become:   x0 = x1 · *i1 , i0 + + y1 · *j1 , i0 + + z1 · *k1 , i0 + y0 = x1 · *i1 , j0 + + y1 · *j1 , j0 + + z1 · *k1 , j0 +  z0 = x1 · *i1 , k0 + + y1 · *j1 , k0 + + z1 · *k1 , k0 + Or in matrix form:       x0 *i1 , i0 + *j1 , i0 + *k1 , i0 + x1  y0  =  *i1 , j0 + *j1 , j0 + *k1 , j0 +  ·  y1  z0 *i1 , k0 + *j1 , k0 + *k1 , k0 + z1 2 34 5 2 34 5 2 34 5 =u|R

R

=RR1

0

0

=u|R

[1.7]

1

1 We can see a rotation matrix RR R0 whose columns are the coordinates of i1 , j1 , k1 expressed in the absolute system R0 . Thus,

1 RR R0

7 7 7  7 i 1 |R 0 7 = 7 7 7

7 7 7 7 j1 |R0 7 7 7 7

7 7 7 7 7 7 7 k 1 |R 0 7  7 7 . 7 7 7 7 7 7

1 This matrix depends on time and links the frame R1 to R0 . The matrix RR R0 is often referred to as a direction cosine matrix since its components involve the direction

10

Mobile Robotics

cosines of the basis vectors of the two coordinate systems. Likewise, if we had several systems R0 , . . . , Rn (see Figure 1.4), we would have: R2 Rn 1 u|R0 = RR R0 · RR1 · . . . · RRn−1 · u|Rn .

Dead Reckoning. Let us consider the situation of a robot moving in a three-dimensional environment. Let us call R0 : (o0 , i0 , j0 , k0 ) its reference frame (for example, the frame of the robot at an initial time). The position of the robot is represented by the vector p(t) expressed in R0 and its attitude (i.e. its orientation) by the rotation matrix R(t) which represents the coordinates of the vectors i1 , j1 , k1 of the coordinate system R1 of the robot expressed in the coordinate system R0 , at time t. It follows that: 7 7 7 7 7 7 7 7 7 7 7 7 1 7 7 7  R(t) = 7 i1 |R0 7 j1 |R0 7 k1 |R0 77 = RR R0 (t). 7 7 7 7

Figure 1.4. Composition of rotations

This matrix can be returned by a precise attitude unit positioned on the robot. If the robot is also equipped with a Doppler Velocity Log (or DVL) which provides it with its speed vector vr relative to the ground or the seabed, expressed in the coordinate system R1 of the robot, then the speed vector v of the robot satisfies: [1.7]

1 v|R0 = RR R · v|R 23450 2 34 51 2 34 5

p(t) ˙

R(t)

vr (t)

Three-dimensional Modeling

11

or equivalently: [1.8]

p˙ (t) = R(t) · vr (t).

Dead reckoning consists of integrating this state equation from the knowledge of R(t) and vr (t). 1.2. Euler angles 1.2.1. Definition In related literature, the angles proposed by Euler in 1770 to represent the orientation of solid bodies in space are not uniquely defined. We mainly distinguish between the roll-yaw-roll, roll-pitch-roll and roll-pitch-yaw formulations. It is the latter that we will choose since it is imposed in the mobile robotics language. Within this roll-pitch-yaw formulation, the Euler angles are sometimes referred to as Cardan angles. Any rotation matrix of R3 can be expressed in the form of the product of three matrices as follows:    cos ψ − sin ψ 0 cos θ 0 sin θ 1 0  R (ϕ, θ, ψ) =  sin ψ cos ψ 0 · 0 0 0 1 − sin θ 0 cos θ 2 34 52 34 5 Rψ

Rθ





1 0 0 · 0 cos ϕ − sin ϕ  0 sin ϕ cos ϕ 2 34 5 Rϕ

or, in developed form:

 cos θ cos ψ − cos ϕ sin ψ + sin θ cos ψ sin ϕ sin ψ sin ϕ + sin θ cos ψ cos ϕ  cos θ sin ψ cos ψ cos ϕ + sin θ sin ψ sin ϕ − cos ψ sin ϕ + sin θ cos ϕ sin ψ  . [1.9] − sin θ cos θ sin ϕ cos θ cos ϕ 

% &' ( i 1 | R0

%

&'

j1 | R0

(

%

&'

k1 | R 0

(

The angles ϕ, θ, ψ are the Euler angles and are respectively called the bank, the elevation and the heading. The terms roll, pitch, yaw are often employed, although they correspond, respectively, to variations of bank, elevation and heading.

12

Mobile Robotics

Gimbal lock. When θ = 

π 2

(the same effect happens as soon as cos θ = 0), we have

     0 0 0 1 1 0 0 0  · 0 1 0  ·  0 cos ϕ − sin ϕ  1 −1 0 0 0 sin ϕ cos ϕ &' ( %   0 sin ϕ cos ϕ   = cos ϕ − sin ϕ   0  −1 0 0 (%     ' & cos ψ − sin ψ 0 cos ϕ sin ϕ 0 0 0 1 =  sin ψ cos ψ 0  ·  − sin ϕ cos ϕ 0  ·  0 1 0  0 0 1 0  0 −1   −1 0 0 cos (ψ − ϕ) − sin (ψ − ϕ) 0 0 0 1 =  sin (ψ − ϕ) cos (ψ − ϕ) 0   0 1 0  0 0 −1 −1 0 0

cos ψ R (ϕ, θ, ψ) =  sin ψ 0

− sin ψ cos ψ 0

and thus, dR dR =− . dψ dϕ This corresponds to a singularity which tells us that when θ = π2 , we cannot move on the manifold of rotation matrices SO(3) in all directions using the Euler angles. Equivalently, this means that some trajectories R(t) cannot be followed by Euler angles. Rotation matrix to Euler angles. Given a rotation matrix R, we can easily find the three Euler angles by solving, following equation [1.9], the equations:   − sin θ = r31 cos θ sin ϕ = r32 cos θ cos ϕ = r33  cos θ cos ψ = r11 cos θ sin ψ = r21

By imposing θ ∈ [− π2 , π2 ], ϕ ∈ [−π, π] and ψ ∈ [−π, π], we find: θ = − arcsin r31 , ϕ = atan2(r32 , r33 ) and ψ = atan2(r21 , r11 ). Here atan2 is the two-argument arctangent function defined by ! x = r cos α α = atan2 (y, x) ⇔ α ∈] − π, π] and ∃r > 0 | y = r cos α

[1.10]

Three-dimensional Modeling

13

1.2.2. Rotation vector of a moving Euler matrix Let us consider a solid body moving in a coordinate system R0 and a coordinate system R1 attached to this body (refer to Figure 1.5). The conventions chosen here are those of the SNAME (Society of Naval and Marine Engineers). The two coordinate systems are assumed to be orthonormal. Let R(t) = R(ψ(t), θ(t), ϕ(t)) be the rotation matrix that links the two systems. We need to find the instantaneous ˙ ψ. ˙ rotation vector ω of the solid body relative to R0 in function of ϕ, θ, ψ, ϕ, ˙ θ, We have: ) * [1.2] ˙ · RT ω|R0 = Ad−1 R +d , [1.9] = Ad−1 )) (Rψ · Rθ · Rϕ ) · RTϕ · RTθ · RTψ dt * * ˙ ψ · Rθ · Rϕ + Rψ · R ˙ θ · Rϕ + Rψ · Rθ · R ˙ ϕ · RTϕ · RTθ · RTψ = Ad−1 R ) * ˙ ψ · RTψ + Rψ · R ˙ θ · RTθ · RTψ + Rψ · Rθ · R ˙ ϕ · RTϕ · RTθ · RTψ = Ad−1 R ) ) * * [1.2] ˙ ˙ = Ad−1 ψAd(k) + Rψ · θAd(j) · RTψ + Rψ · Rθ · (ϕAd(i)) ˙ · RTθ · RTψ ) * [1.3] = Ad−1 ψ˙ · Ad(k) + θ˙ · Ad(Rψ · j) + ϕ˙ · Ad(Rψ · Rθ · i) = ψ˙ · k + θ˙ · Rψ · j + ϕ˙ · Rψ · Rθ · i.

Figure 1.5. The coordinate system R1 : (o1 , i1 , j1 , k1 ) attached to the robot

Thus, after having calculated the quantities k, Rψ j and Rψ ·Rθ ·i in the coordinate system R0 , we have: ω|R0

      0 − sin ψ cos θ cos ψ = ψ˙ ·  0  + θ˙ ·  cos ψ  + ϕ˙ ·  cos θ sin ψ  . 1 0 − sin θ

From this we get the result:    ϕ˙ cos θ cos ψ − sin ψ 0    θ˙  . ω|R0 = cos θ sin ψ cos ψ 0 − sin θ 0 1 ψ˙

[1.11]

14

Mobile Robotics

Note that this matrix is singular if and only if cos θ = 0. We will therefore take care to never have an elevation θ equal to ± π2 , to avoid the gimbal lock. 1.3. Inertial unit Assume that we are enclosed inside a box near to the Earth without any ability to see the environment outside. Inside the box we may perform some inertial experiments such as observing bodies translating or rotating. These experiments allow us to indirectly measure the tangential accelerations and the angular speed of the box in its own frame. The principle of an inertial unit is to estimate the pose (position and orientation) of the box from these inertial measurements only (called proprioceptive measurements), assuming that the initial pose is known. For this purpose, we will describe the motion of our box with a kinematic model, the inputs of which are the tangential accelerations and the angular speeds, which are directly measurable in the coordinate system of the box. The state vector is composed of the vector p = (px , py , pz ) that gives the coordinates of the center of the box expressed in the absolute inertial coordinate system R0 , the three Euler angles (ϕ, θ, ψ) and the speed vector vr of the box expressed in its own coordinate system R1 . The inputs of the system are first the acceleration ar = aR1 of the center of the box also given in R1 and second, the vector ω r = ω R1 /R0 |R1 = (ωx , ωy , ωz ) corresponding to the rotation vector of the box relative to R0 expressed in R1 . We have to express a, ω in the coordinate system of the box since these quantities are generally measured via the sensors attached on it. The first state equation is: [1.8]

p˙ = R (ϕ, θ, ψ) · vr . Let us differentiate this equation. We obtain: ˙ · vr + R · v˙ r p ¨=R with R = R (ϕ, θ, ψ). From this equation, we isolate v˙ r to get [1.4]

˙ · v r = ar − ω r ∧ v r , v˙ r = RT · p ¨ − RT R 2 34 5 ar

which constitutes the second state equation. Figure 1.6 shows a situation where a robot has a constant speed and follows a circle. The speed vector vr is a constant whereas ar is different from zero.

Three-dimensional Modeling

15

Figure 1.6. Illustration of the formula v˙ r = ar − ω r ∧ vr when v˙ r = 0. Left: the robot follows a circle; right: representation of the vectors in the robot frame

We also need to express equation [1.11], the relation

-

˙ ψ˙ ϕ, ˙ θ,

.

in function of the state variables. From

ω |R0 = R (ϕ, θ, ψ) · ω |R1 becomes   ϕ˙ cos θ cos ψ − sin ψ 0  cos θ sin ψ cos ψ 0   θ˙  = R (ϕ, θ, ψ) · ω r . − sin θ 0 1 ψ˙ 

. ˙ ψ˙ in this expression, we obtain the third state By isolating the vector ϕ, ˙ θ, equation. By bringing together the three state equations, we obtain the navigation mechanization equations [FAR 08]:  p˙ = R (ϕ, θ, ψ) · vr     ˙ v = ar − ω r ∧ vr  r    ϕ˙ 1 tan θ sin ϕ tan θ cos ϕ [1.12]    θ˙ =  0 cos ϕ − sin ϕ  · ω r    cos ϕ sin ϕ 0 ψ˙ cos θ cos θ

On a horizontal plane. If we move on a horizontal plane, we have ϕ = θ = 0. The last equation gives us ψ˙ = ωr3 , θ˙ = ωr2 and ϕ˙ = ωr1 . In such a case, there is a perfect correspondence between the components of ω r and the differentials of the Euler angles. There are singular cases, for instance when θ = π2 (this is the case when the box points upwards), in which the differentials of the Euler angles cannot

16

Mobile Robotics

be defined. Using the rotation vector is often preferred since it does not have such singularities. Dead reckoning. For dead reckoning (i.e. without external sensors) there are generally extremely precise laser gyrometers (around 0.001 deg/s.). These make use of the Sagnac effect (in a circular optical fiber turning around itself, the time taken by light to travel an entire round-trip depends on the path direction). Using three fibers, these gyrometers generate the vector ω r = (ωx , ωy , ωz ). There are also accelerometers capable of measuring the acceleration ar with a very high degree of precision. In pure inertial mode, we determine our position by differentiating equations [1.12] only using the acceleration ar and the rotation speed ω r , both expressed in the coordinate system of the box. In the case where we are measuring the quantity vr (also expressed in the frame of the inertial unit attached to the robot) with a Doppler velocity log, we only need to integrate the first and the last of these three equations. Finally, when the robot is a correctly ballasted submarine or a terrestrial robot moving on a relatively plane ground, we know a priori that on average the bank and the elevation are equal to zero. We may therefore incorporate this information through a Kalman filter in order to limit the drift in positioning. An efficient inertial unit integrates an amalgamation of all the available information. Inertial unit. A pure inertial unit (without hybridization and without taking into account Earth’s gravity) represents the robot by the kinematic model of Figure 1.7, which itself uses the state equations given in [1.12]. This system is written in the form x˙ = f (x, u) where u = (ar , ω r ) is the vector of the measured inertial inputs (accelerations and rotation speeds viewed by an observer on the ground, but expressed in the frame of the robot) and x = (p, vr , ϕ, θ, ψ) is the state vector. We may use a numerical integration method such as the Euler method. This leads to replacing the differential equation x˙ = f (x, u) with the recurrence: x (t + dt) = x (t) + dt · f (x (t) , u (t)) .

Figure 1.7. Navigation mechanization equations

Three-dimensional Modeling

17

With gravity. In the case where there exists a gravity g (p) which depends on p, the actual acceleration ar corresponds to the gravity plus the measured acceleration ames . In this case, the inertial unit can be described by the following state equations: r  p˙ = R     (ϕ, θ, ψ) · vr   ϕ˙   1 tan θ sin ϕ tan θ cos ϕ   θ˙  =  0 cos ϕ − sin ϕ  · ω r [1.13]  cos ϕ sin ϕ  ˙ 0  ψ  cos θ cos θ  v˙ r = RT (ϕ, θ, ψ) · g (p) + ames − ω r ∧ vr r

In this case, the inputs of the system are ames and ω r which are obtained by r the accelerometers and the gyroscope. To integrate this state equation for localization purposes, we need to know the initial state and a gravity map g(p). Using rotation matrix instead of Euler angles. From equation [1.4], we have / 0 ˙ = R · Ad RT ω . Thus, the state equations [1.13] can be written without any R Euler angles as   p˙ = R · vr ˙ = R · (ω r ∧ ) [1.14] R  v˙ r = RT · g (p) + ames − ω ∧ v r r r The main advantage of this representation is that we do not have the singularity that exists in [1.13] for cos θ = 0. But instead, we have redundancies, since R of dimension 9 replaces the three Euler angles. For numerical reasons, when we integrate for a long time, the matrix R(t) may loose the property R · RT = I. To avoid this, at each iteration, a renormalization is needed, i.e. the current matrix R should be projected onto SO(3). One possibility is to use a QR factorization. A QR-decomposition of a square matrix M provides two matrices Q,R such that M = Q · R where Q is a rotation matrix and R is triangular. When M is almost a rotation matrix, R is almost diagonal and also almost a rotation, i.e. on the diagonal of R the entries are approximately ±1 and outside the entries are almost zeros. The following P YTHON code performs the projection of M on SO(3) and generates the rotation matrix M2 . Q,R = numpy.linalg.qr(M) v=diag(sign(R)) M2=Q@diag(v) 1.4. Dynamic modeling 1.4.1. Principle A robot (airplane, submarine, boat) can often be considered as a solid body whose inputs are the (tangential and angular) accelerations. These quantities are

18

Mobile Robotics

analytic functions of the forces that are at the origin of the robot’s movement. For the dynamic modeling of a submarine the reference work is Fossen’s book [FOS 02], but the related notions can also be used for other types of solid robots such as planes, boats or quadrotors. In order to obtain a dynamic model, it is sufficient to take the kinematic equations and to consider the angular and tangential accelerations caused by forces and dynamic performance. These quantities become the new inputs of our system. The link between the accelerations and the forces is done by Newton’s second law (or the fundamental principle of dynamics). Thus, for instance if f is the net force resulting from the external forces expressed in the inertial frame and m is the mass of the robot, we have m¨ p = f. Since the speed and accelerations are generally measured by sensors embedded in the system, we generally prefer to express the speed and accelerations in the frame of the robot: mar = fr , where fr is the applied forces vector expressed in the robot frame. The same type of relation, known as the Euler’s rotation equation, exists for rotations. It is given by Iω˙ r + ω r ∧ (Iω r ) = τ r

[1.15]

where τ r is the applied torque and ω r is the rotation vector, both expressed in the robot frame. The inertia matrix I is attached to the robot (i.e. computed in the robot frame) and we generally choose the robot frame to make I diagonal. This relation is a consequence of Euler’s second law which states that in an inertial frame, the time derivative of the angular momentum is equal to the applied torque. In the inertial frame, this can be expressed as d dt (R · I · ω r ) = R · τ r ˙ ⇔ RI · ω r + R · I · ω˙ r = R · τ r ˙ · ω r + I · ω˙ r = τ r ⇔ RT RI ⇔ ω r ∧ (Iω r ) + Iω˙ r = τ r .

1.4.2. Modeling a quadrotor As an illustration, we consider a quadrotor (see Figure 1.8) for which we want a dynamic model. The robot has four propellers that can be tuned independently. This will allow us to control the attitude and position of the robot by changing the speeds of the motors.

Three-dimensional Modeling

19

Figure 1.8. Quadrotor. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

We distinguish the front/rear propellers (blue and black), rotating clockwise and right/left propellers (red and green), rotating counterclockwise. The value of the force generated by the ith propeller is proportional to the squared speeds of the rotors, i.e. is equal to β · ωi · |ωi |, where β is the thrust factor. Denote by δ the drag factor and * the distance between any rotor and the center of the robot. The forces and torques generated on the robot are       τ0 β β β β ω1 · |ω1 |  τ1   −β* 0 β* 0   ω2 · |ω2 |   =     τ2   0 −β* 0 β*  ·  ω3 · |ω3 |  τ3 −δ δ −δ δ ω4 · |ω4 | where τ0 is the total thrust generated by the rotors and τ1 , τ2 , τ3 are the torques generated by differences in the rotor speeds. We will now show that the state equations for the quadrotor are given by  (i)    p˙  = R  (ϕ, θ, ψ) · vr    ϕ ˙ 1 tan θ sin ϕ tan θ cos ϕ      θ˙  =  0 cos ϕ − sin ϕ  · ω r (ii)    sin ϕ cos ϕ  ˙ 0  ψ  cos θ  cos θ   0 0  v˙ r = RT (ϕ, θ, ψ) ·  0  +  0  − ω r ∧ vr (iii)     − τm0     g    τ1   −1    ˙ τ2  − ω r ∧ (I · ω r ) (iv) ω = I · r   τ3

20

Mobile Robotics

where p is the position and (ϕ, θ, ψ), are the Euler angles of the robot. The first three equations (i), (ii), (iii) correspond to the kinematic equations and have already been derived (see equation [1.12]). Note that in equation (iii), the acceleration ar (expressed in the robot frame) comes from Newton’s second law where the forces are the total force τ0 and the weight mg: 

   0 0 mar = RT (ϕ, θ, ψ) ·  0  +  0  . mg −τ0

Since the gravity vector is expressed naturally in the inertial frame (contrary to the global force τ0 ), we have to multiply the gravity vector by the Euler rotation matrix RT (ϕ, θ, ψ). Equation (iv) comes from Euler’s rotation equation [1.15], ω˙ r = I−1 · (τ r − ω r ∧ (I · ω r )) where τ r = (τ1 , τ2 , τ3 ) is the torque vector. 1.5. Exercises E XERCISE 1.1.– Properties of the adjoint matrix Let us consider the vector ω = (ωx , ωy , ωz ) and its adjoint matrix Ad (ω). 1) Show that the eigenvalues of Ad (ω) are {0, -ω- · i, −-ω- · i}. Give an eigenvector associated with 0. Discuss. 2) Show that the vector Ad (ω) x = ω ∧ x is a vector perpendicular to ω and x, such that the trihedron (ω, x, ω ∧ x) is direct. 3) Show that the norm of ω ∧ x is the surface of the parallelogram A mediated by ω and x. E XERCISE 1.2.– Jacobi identity The Jacobi identity is written as: a ∧ (b ∧ c) + c ∧ (a ∧ b) + b ∧ (c ∧ a) = 0. 1) Show that this identity is equivalent to: Ad (a ∧ b) = Ad (a) Ad (b) − Ad (b) Ad (a) where Ad (ω) is the adjoint matrix of the vector ω ∈ R3 .

Three-dimensional Modeling

21

2) In the space of skew-symmetric matrices, the Lie bracket is defined as follows: [A, B] = A · B − B · A. Show that: Ad (a ∧ b) = [Ad (a) , Ad (b)]. 3) In the exercise 1.4, we will show that an infinitesimal rotation following the 2 skew symmetric matrix A is exp (A · dt) . I + A · dt + A2 · dt2 + ◦(dt2 ). Consider two skew-symmetric matrices A, B and define the two infinitesimal rotation matrices Ra = eA·dt and Rb = eB·dt . Compute a Taylor expansion of second order associated with the rotation −1 R−1 a Rb Ra Rb .

From this result, give an interpretation of the Lie bracket [A, B] = AB − BA. / 0 4) Verify that the set R3 , +, ∧, · forms a Lie algebra.

E XERCISE 1.3.– Varignon’s formula

Let us consider a solid body whose center of gravity remains at the origin of a Galilean coordinate system and is rotating around an axis ∆ with a rotation vector of ω. Give the equation of the trajectory of a point x of the body. E XERCISE 1.4.– Rodrigues’ formula Let us consider a solid body whose center of gravity remains at the origin of a Galilean coordinate system and is rotating around an axis ∆. The position of a point x of the body satisfies the state equation (Varignon’s formula): x˙ = ω ∧ x where ω is parallel to the axis of rotation ∆ and ||ω|| is the rotation speed of the body (in rad.s−1 ). 1) Show that this state equation can be written in the form: x˙ = Ax.

22

Mobile Robotics

Explain why the matrix A is often denoted by ω∧. 2) Give the expression of the solution of the state equation. 3) Deduce from this that the expression of the rotation matrix R with angle ||ω|| around ω is given by the following formula, referred to as Rodrigues’ formula: R = eω∧ . 4) Calculate the eigenvalues of A and show that ω is the eigenvector associated with the zero eigenvalue. Discuss. 5) What are the eigenvalues of R? 6) Using the previous questions, give the expression of a rotation around the vector ω = (1, 0, 0) of angle α. 7) Write an expression of the Euler matrix that uses Rodrigues’ formula. E XERCISE 1.5.– Geometric approach to Rodrigues’ formula Let us consider the rotation Rn,ϕ of angle ϕ around the unit vector n. Let u be a vector that we will subject to this rotation. The vector u can be decomposed as follows: u =< u, n > ·n + u− < u, n > ·n 2 34 5 2 34 5 u||

u⊥

where u|| is collinear to n and u⊥ is in the plane P⊥ orthogonal to n (see Figure 1.9). 1) Prove Rodrigues’ formula given by: Rn,ϕ (u) =< u, n > ·n + (cos ϕ) (u− < u, n > ·n) + (sin ϕ) (n ∧ u) . / 0 / 0 2) Using the double cross product formula a∧ (b ∧ c) = aT c · b − aT b · c, on the element n∧ (n ∧ u), show that Rodrigues’ formula can also been written as: Rn,ϕ (u) = u + (1 − cos ϕ) (n∧ (n ∧ u)) + (sin ϕ) (n ∧ u) . as:

Deduce from this that the matrix associated with the linear operator Rn,ϕ is written

Rn,ϕ



   −n2y − n2z nx ny nx nz 1 0 0 −n2x − n2z ny nz  =  0 1 0  + (1 − cos ϕ)  nx ny 2 2 0 0 1 n n n n −n x z y z x − ny   0 −nz ny 0 −nx  . + (sin ϕ)  nz −ny nx 0

Three-dimensional Modeling

23

Figure 1.9. Rotation of the vector u around the vector n; Left: perspective view; right: view from above

3) Conversely, we are given a rotation matrix Rn,ϕ for which we wish to find the axis of rotation n and the angle of rotation ϕ. Compute the trace tr (Rn,ϕ ) and Rn,ϕ − RTn,ϕ and use it to obtain n and ϕ in function of Rn,ϕ . For a geometric illustration, see Figure 1.10.

Figure 1.10. Left: a view of the rotation of angle ϕ around n; right: a visualization of the section corresponding to the Rodrigues rhombus

24

Mobile Robotics

4) Deduce a method to find a interpolation trajectory matrix R(t) between two rotation matrices Ra , Rb such that R(0) = Ra and R(1) = Rb . 5) Using a Maclaurin series development of sinϕ and cosϕ, show that: Rn,ϕ = exp (ϕ · Ad (n)) , which sometimes written as: Rn,ϕ = exp (ϕ · n∧) . E XERCISE 1.6.– Quaternions Quaternions, discovered by W.R. Hamilton, are also used to represent a rotation in a three-dimensional space (see, e.g. [COR 11]). A quaternion ˚ q is an extension of the complex number. It corresponds to a scalar s plus a vector v of R3 . We use the following equivalent notations: ˚ q = s + v1 i + v2 j + v3 k = s+ < v1 , v2 , v3 >= s+ < v >, where i2 = j 2 = k 2 = ijk = −1. 1) From these relations, fill the following multiplication table: · 1 i j k

1 1 i j k

i i ? ? ?

j j ? ? ?

k k ? ? ?

2) A unit-quaternion ˚ q is a quaternion with a unit magnitude: |˚ q |2 = s2 + v12 + v22 + v32 = 1. Consider the rotation obtained by a rotation around the unit vector v with an angle θ. From the Rodrigues formula, we know that the corresponding rotation matrix is exp(θ · v∧). To this rotation, we associate the quaternion: θ θ ˚ q = cos( ) + sin( )· < v > . 2 2 Check that this quaternion has a unit magnitude.

Three-dimensional Modeling

25

3) Show that the quaternion ˚ q and its opposite −˚ q both correspond to the same rotation in R3 . 4) Assuming that the composition of rotations corresponds to a multiplication of quaternions, show that (s+ < v >)

−1

= s+ < −v > .

5) We consider the Euler rotation matrix with ψ = θ = ϕ = π2 . It corresponds to a rotation around the unit vector v with an angle α. Give the values of v and α using three methods: the geometrical method (using hands only), the matrices and the quaternions. E XERCISE 1.7.– Schuler pendulum One of the fundamental components of an inertial unit is the inclinometer. This sensor gives the vertical direction. Traditionally, we use a pendulum (or a plumb line) for this. However, when we are moving, due to the accelerations the pendulum starts to oscillate and it can no longer be used to measure the vertical direction. Here, we are interested in designing a pendulum for which any horizontal acceleration does not lead to oscillations. Let us consider a pendulum with two masses m at each end, situated at a distance *1 and *2 from the axis of rotation of the rod (see Figure 1.11). The axis moves over the surface of the Earth. We assume that *1 and *2 are small in comparison to Earth’s radius r.

Figure 1.11. Inclinometer pendulum moving over the surface of the Earth

26

Mobile Robotics

1) Find the state equations of the system. 2) Let us assume that α = α˙ = 0. For which values of *1 and *2 does the pendulum remain vertical, for any horizontal movement of the pendulum? What values does *2 have to take if we let *1 = 1 m and we take r = 6 400 km for the Earth’s radius? 3) Let us assume that, as a result of disturbances, the pendulum starts to oscillate. The period of these oscillations is called the Schuler period. Calculate this period. 4) Simulate the system graphically by taking a Gaussian white noise as the acceleration input. Since the system is conservative, an Euler integration method will not perform well (the pendulum would gain energy). A higher-order integration scheme, such as that of Runge Kutta, should be used. This is given by: x(t + dt) . x(t) + dt " # f (x(t), u(t)) 3 2dt · + 4 f (x(t) + f (x(t), u(t)), u(t + 23 dt)) . 4 3 For an easier graphical representation, we will take r = 10 m, *1 = 1 m and g = 10 ms−2 . Discuss the results. E XERCISE 1.8.– Brake detector We will now look at a problem that involves basis changes and rotation matrices. A car is preceded by another car m (which we will assume to be a point). We attach to this car the coordinate system R1 : (o1 , i1 , j1 ) as represented in Figure 1.12. The coordinate system R0 : (o0 , i0 , j0 ) is a ground frame assumed to be fixed. This car is equipped with the following sensors: – several odometers placed on the rear wheels, allowing us to measure the speed v of the center of the rear axle; ¨ θ;

˙ as well as the angular acceleration – a gyro giving the angular speed of the car θ,

– an accelerometer placed at o1 , allowing us to measure the acceleration vector (α, β) of o1 expressed in the coordinate system R1 ;

– using two radars placed at the front, our car is capable of (indirectly) measuring the coordinates (a1 , b1 ).of the-point m . in the coordinate system R1 as well as the first ˙ ¨ two derivatives a˙ 1 , b1 and a ¨ 1 , b1 .

Three-dimensional Modeling

27

On the other hand, the car is not equipped with a positioning system (such as a GPS, for example) that would allow it to gain knowledge of x, y, x, ˙ y. ˙ It does not have a compass for measuring the angle θ. The quantities in play are the following: ˙ a1 , b1 , α, β. Measured v, θ, Unknown x, y, x, ˙ y, ˙ θ, a0 , b0 . When a quantity is measured, we assume that its differentials are also measured, but not their primitive. For example, a˙ 1 , b˙ 1 , a ¨1 , ¨b1 are considered to be measured since a1 , b1 are measured. However, θ˙ is measured but θ isn’t. We do not know the state equations of our car. The goal of this problem is finding a condition on the measured variables (and their derivatives) that will allow us to tell whether the point m is braking or not. We understand that such a condition would allow us to build a warning system informing us that the preceding vehicle is braking, even when its rear brake lights are not visible (fog, trailer without brake lights, etc.) or defective.

Figure 1.12. Car trying to detect whether the point m is braking

1) By expressing the Chasle relation (o0 m = o0 o1 + o1 m) in the coordinate system R0 , show the basis change formula: "

a0 b0

#

=

" # " # x a1 + Rθ y b1

where Rθ is a rotation matrix.

28

Mobile Robotics

˙ θ is a skew-symmetric matrix and find its expression. 2) Show that RTθ R 3) Let u be the speed vector of the point m viewed by a fixed observer. Find an expression u|R1 of the speed vector u expressed in the coordinate system R1 . Express ˙ a1 , a˙ 1 , b1 , b˙ 1 . u|R1 in function of the measured variables v, θ,

4) We will now use the accelerometer of our vehicle, which gives us the acceleration vector (α, β) of o1 , expressed in R1 . By differentiating m|R0 two times, give the expression of the acceleration a|R0 of m in the coordinate system R0 . Deduce from this its expression a|R1 in the coordinate system R1 . Give the expression of a|R1 only in function of the measured variables. ˙ a1 , b1 , a˙ 1 , b˙ 1 , a 5) Find a condition on the measurements (v, θ, ¨1 , ¨b1 , α, β) which allows us to detect whether the vehicle in front is braking. E XERCISE 1.9.– Modeling an underwater robot The robot we will be modeling is the Redermor (greyhound of the sea in the Breton language). A photograph is shown in Figure 1.13. It is an entirely autonomous underwater robot. This robot, developed by GESMA (Groupe d’Etude Sous-Marine de l’Atlantique – Atlantic underwater research group), has a length of 6 m, a diameter of 1 m and a weight of 3,800 kg. It has an efficient propulsion and control system with the aim of finding mines on the seabed.

Figure 1.13. Redermor built by GESMA ( Groupe d’Etude Sous-Marine de l’Atlantique – Atlantic underwater research group), on the water surface, still close to the boat it was launched from

Let us build a local coordinate system R0 : (o0 , i0 , j0 , k0 ) over the area traveled by the robot. The point o0 is placed on the surface of the ocean. The vector i0 indicates north, j0 indicates east and k0 is oriented towards the center of the Earth.

Three-dimensional Modeling

29

Let p = (px , py , pz ) be the coordinates of the center of the robot expressed in the coordinate system R0 . The state variables of the underwater robot are its position p in the coordinate system R0 , its tangential v and its three Euler angles ϕ, θ, ψ. Its inputs are the tangential acceleration v˙ as well as three control surfaces which act respectively on ωx , ωy , ωz . More formally, we have:  u1    vu2 vu3    u4

= = = =

v˙ ωy ωz ωx

where the factor v preceding u2 , u3 indicates that the robot is only able to turn left/right (through u3 ) or up/down (through u2 ) when it is advancing. Give the kinematic state model for this system. E XERCISE 1.10.– 3D robot graphics Drawing two- or three-dimensional robots or objects on the screen is widely used for simulation in robotics. The classic method (used by O PEN GL) relies on modeling the posture of objects using a series of affine transformations (rotations, translations, homotheties) of the form: fi :

Rn → Rn x 0→ Ai x + bi

with n = 2 or 3. However, the manipulation of compositions of affine functions is less simple than that of linear applications. The idea of the transformation in homogeneous coordinates is to transform a system of affine equations into a system of linear equations. Notice first of all that an affine equation of the type y = Ax + b can be written as: " # " #" # y A b x = . 1 0 1 1 We will thus define the homogeneous transformation of a vector as follows: x 0→ xh =

"

x 1

#

.

Thus, an equation of the type y = A3 (A2 (A1 x + b1 ) + b2 ) + b3

30

Mobile Robotics

involving the composition of three affine transformations, can be written as: yh =

"

A3 b 3 0 1

#"

A2 b 2 0 1

#"

A1 b 1 0 1

#

xh .

A pattern is a matrix with two or three rows (depending on whether the object is in the plane or in space) and n columns representing the n vertices of a rigid polygon embodying the object. It is important that the union of all the segments formed by two consecutive points of the pattern forms all the vertices of the polygon that we wish to represent. 1) Let us consider the underwater robot (or AUV, short for Autonomous Underwater Vehicle) whose pattern matrix in homogeneous coordinates is:   0 0 10 0 0 10 0 0  −1 1 0 −1 −0.2 0 0.2 1  . M=  0 0 0 0 1 0 1 0 1 1 1 1 1 1 1 1 Draw this pattern in perspective view on a piece of paper.

2) The state equations of the robot are the following:  p˙ x = v cos θ cos ψ     p ˙ y = v cos θ sin ψ     p  ˙ z = −v sin θ v˙ = u1   ϕ˙ = −0.1 sin ϕ · cos θ + tan θ · v · (sin ϕ · u2 + cos ϕ · u3 )     θ˙ = cos ϕ · v · u2 − sin ϕ · v · u3    ˙ ϕ cos ϕ ψ = sin cos θ · v · u2 + cos θ · v · u3

where (ϕ, θ, ψ) are the three Euler angles. The inputs of the system are the tangential acceleration u1 , the pitch u2 and the yaw u3 . The state vector is therefore equal to x = (px , py , pz , v, ϕ, θ, ψ). Give a function capable of drawing the robot in 3D together with its shadow, in the plane x-y. Verify that the drawing is correct by moving the six degrees of freedom of the robot one by one. Obtain a three-dimensional representation such as the one illustrated in Figure 1.14. 3) By using the relation  cos θ cos ψ ω|R0 =  cos θ sin ψ − sin θ

− sin ψ cos ψ 0

  ϕ˙ 0 0   θ˙  , 1 ψ˙

draw the instantaneous rotation vector of the robot. 4) Simulate the robot in various conditions using a Euler method.

Three-dimensional Modeling

31

Figure 1.14. 3D representation of the robot together with its shadow in the horizontal plane

E XERCISE 1.11.– Manipulator robot

Figure 1.15. Parametrization for the direct geometric model of a manipulator robot

A manipulator robot, such as Staubli (represented on Figure 1.15), is composed of several rigid arms. We retrieve the coordinates of the end effector, at the extremity of the robot, using a series of geometric transformations. We can show that a parametrization with four degrees of freedom allows us to represent these transformations. There are several possible parametrizations, each with its own advantages and disadvantages. The most widely used one is probably the Denavit–Hartenberg parametrization. In the case where the articulations are rotational joints (as is the case of the Staubli robot where the joins can turn), the parametrization represented by the figure might prove to be practical since it makes it easier to draw the robot. This transformation is the composition of four elementary transformations: (i) a translation of length r following z; (ii) a translation of length d following x; (iii) a rotation of α around y and (iv) a rotation of θ (the variable

32

Mobile Robotics

activated around z). Using the figure for drawing the arms and the photo for the robot, perform a realistic simulation of the robot’s movement. E XERCISE 1.12.– Floating wheel Consider a wheel floating and spinning in the space as shown on Figure 1.16. We assume that the wheel is solid, similar to a homogeneous disk of mass m = 1 and radius ρ = 1. Its inertia matrix is given by: 

mρ2 2

 I = 0 0

0 mρ2 4

0

0 0 mρ 4

2

  

Figure 1.16. Floating wheel. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

1) Give the state equations of the wheel. The state variables we choose are (i) the coordinates p = (x, y, z) of the center of the wheel, (ii) the orientation (ϕ, θ, ψ) of

Three-dimensional Modeling

33

the wheel, (iii) the speed vr of the center of the wheel in the wheel frame, and (iv) the rotation vector ω r expressed in the wheel frame. We thus have a system of order 12. 2) The torque-free precession occurs when the angular velocity vector changes its orientation with time. Provide a simulation illustrating the precession. 3) Show that the angular momentum L = R · I · RT · ω and the kinetic energy EK = 12 LT · ω are both constant. Verify this with a simulation.

4) Using symbolic computation, compute the Jacobian matrix of the evolution function f at point T

x0 = (0, 0, 0, 0, 0, 0, 0, 0, 0, ωx , ωy , ωz ) . Give an interpretation. E XERCISE 1.13.– Schuler oscillations in an inertial unit Consider the Earth as static, without rotation, with a gravity field directed toward the center and with a magnitude g = 9.81ms−2 . An inertial unit is fixed in a robot R1 at the surface of Earth of radius r. Its position is with p = (r, 0, 0) and its Euler angles rate is (ϕ, θ, ψ) = (0, 0, 0) as in Figure 1.17. The robot R1 will always be static in the exercise.

Figure 1.17. The inertial unit inside the robot is static

34

Mobile Robotics

1) Give the values for ames and ω r collected by the inertial unit of R1 . r

2) We now want to lure the inertial unit by initializing the robot at some other place, say R2 , and finding a trajectory so that the unit believes that it is fixed in R1 . More precisely, we want to find a motion p(t) = (x(t), y(t), z(t)) for R2 so that the inertial unit senses exactly the inputs ames , ω r of R1 . For this, we assume that at time r t = 0, (ϕ, θ, ψ) = (0, 0, 0) for R2 . For simplicity, we also assume that R2 remains in the plane y = 0. Simplify the state equations for R2 in our specific case. T

3) Provide a simulation of the trajectory of R2 , initialized at p(0) = (r, 0, 1) . Interpret. 4) Linearize the state equation for R2 at the state vector corresponding to R1 . Compute the eigenvalues and give an interpretation.

5) Assuming that the inertial unit is not perfectly initialized, what can you conclude for the corresponding error propagation? E XERCISE 1.14.– Lie bracket for control Consider two vector fields f and g of Rn corresponding to two state equations x˙ = f (x) and x˙ = g(x). We define the Lie bracket between these two vector fields as [f , g] =

dg df ·f − · g. dx dx

We can check that the set of vector fields equipped with the Lie bracket is a Lie algebra. The proof is not difficult but it is tedious and will not be demanded in this exercise. 1) Consider the linear vector fields f (x) = A·x, g(x) = B·x. Compute [f , g] (x). 2) Consider the system x˙ = f (x) · u1 + g(x) · u2 . We propose to apply the following cyclic sequence: t ∈ [0, δ] t ∈ [δ, 2δ] t ∈ [2δ, 3δ] t ∈ [3δ, 4δ] t ∈ [4δ, 5δ] . . . u = (1, 0) u = (0, 1) u = (−1, 0) u = (0, −1) u = (1, 0) . . . where δ is an infinitesimal time period δ . This periodic sequence will be denoted by {(1, 0) , (0, 1) , (−1, 0) , (0, −1)} Show that / 0 x(t + 2δ) = x (t − 2δ) + [f , g] (x(t)) δ 2 + o δ 2 .

Three-dimensional Modeling

35

3) Which periodic sequence should we take in order to follow the field ν · [f , g]? We will first consider the case ν ≥ 0 and then the case ν ≤ 0. 4) Propose a controller such that the system becomes x˙ = f (x) · a1 + g(x) · a2 + [f , g] (x) · a3 , where a = (a1 , a2 , a3 ) is the new input vector. 5) Consider a Dubins car described by the following state equations:   x˙ = u1 cos θ y˙ = u1 sin θ ˙ θ = u2

where u1 is the speed of the car, θ its orientation and (x, y) the coordinates of its center. Using Lie brackets, add a new input to the system, i.e. a new direction of control. 6) Propose a simulation to check the good behavior of your controller. To do this, take δ small enough to be consistent with the second order Taylor approximation but √ large with respect to the sampling time dt. For instance, we may take δ = dt. The initial state vector is taken as x(0) = (0, 0, 1). 7) Propose a controller so that the car goes up. Consider the same questions, where the car goes down, right and left, respectively. E XERCISE 1.15.– Follow the equator We consider a mobile body moving around a planet, Earth, with a radius ρE rotating along the vertical axis with a rotation rate ψ˙ E . 1) Draw Earth rotating with a static body at position p with the following frames (see Figure 1.18): i) R0 is the Earth-centered inertial frame, fixed with respect to the stars.

ii) R1 is the Earth frame and turns with the Earth with a rotational speed ψ˙ E .

iii) R2 is the navigation frame, ego-centered and pointing towards the sky. iv) R3 is the ego-centered frame of the robot.

For the simulation, take ψ˙ E = 0.2 rad/s, ρE = 10m. 2) Provide the kinematic state equations for the body. The state variables are taken as (p, R3 , v3 ), where p is the position of the body in R0 , R3 is the orientation of the body, and v3 is the speed of the body seen from R0 and expressed in R3 . The inputs are chosen as ω 3 , the rotation vector of the body and a3 , the measured acceleration,

36

Mobile Robotics

both expressed in R3 . Propose a simulation with Earth rotating and the body moving p in space with a gravity given by g (p) = −9.81 · ρ2T &p&3 .

Figure 1.18. Earth with the different frames. The origin of the frames have been shifted for more visibility. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

3) The planet is now assumed to be made of water and the body is an underwater robot equipped with actuators (such as propellers) to move inside the water. The gravity is taken as g (p) = − 9.81 ρT p. The water of the rotating planet drags the robot through friction forces (tangential and rotational). Moreover, the robot, which has the same density as the surrounding fluid, receives an Archimedes upward buoyant force. Propose a dynamical model describing the motion of the robot. The inputs will be the rotational acceleration uω3 and the tangential acceleration ua3 expressed in R3 . These accelerations result from the action of the actuators creating forces and torques on the robot. Propose a simulation illustrating all these effects. 4) The robot behaves like a 3D Dubins-like car model. More precisely, it is equipped with a single propeller which pushes the robot forward without any possibility of controlling the speed. We take ua3 = (1, 0, 0)T . We assume that the robot can chose its rotation vector using rudders, jet pumps or inertial wheels. Find a controller such that the robot goes east along the equator. Validate this with a simulation.

Three-dimensional Modeling

37

1.6. Corrections C ORRECTION FOR E XERCISE 1.1.– (Properties of the adjoint matrix) 1) The characteristic polynomial of the matrix Ad (ω) is given by: / 0 / / 00 s3 + ωx2 + ωy2 + ωz2 s = s s2 + ωx2 + ωy2 + ωz2 .

From this we obtain the eigenvalues {0, ||ω||i, −||ω||i}. Finally, we have:      0 −ωz ωy ωx 0  ωz 0 −ωx   ωy  =  0  −ωy ωx 0 ωz 0

and therefore the eigenvector associated with 0 is ω. The matrix Ad (ω) is associated with a speed vector field of a turning coordinate system with ω. Since the axis ω does not move, we will have Ad (ω) · ω = 0. 2) (i) We now show that x ⊥ (ω ∧ x). For this, it is sufficient to prove that xT Ad (ω) x = 0. We have: T T 2xT Ad (ω) x = xT Ad (ω) (since they are scalar) / (ω) x + x Ad 0 x T T = x Ad (ω) + Ad (ω) x =0 (since Ad (ω) is skew symmetric)

(ii) Since ω T Ad (ω) x = xT AdT (ω) ω = −xT Ad (ω) ω = 0

(Ad (ω) is skew symmetric) (ω is the 0-eigenvector of the Ad (ω)

we get that ω ⊥ (ω ∧ x). (iii) It is easily shown that: 7 7 7 7 7 7 7 7 7 7 7 7 7 ω 7 x 7 ω ∧ x 7 7 7 7 7 = -ω ∧ x-2 . det  7 7 7 7 7 7 7 7 7 7 7 7

For this, we need to develop the two expressions above and verify their equality. The positivity of the determinant implies that the trihedron (ω, x, ω ∧ x) is direct. 3) The parallelepiped carried by ω, x and ω ∧ x has a volume of: 7 7 7 7 7 7 7 7 7 7 7 7 7 ω 7 x 7 ω ∧ x 7 7 = -ω ∧ x-2 . 7 7 7  v = det 7 7 7 7 7 7 7 7 7 7 7 7

38

Mobile Robotics

However, since ω ∧ x is orthogonal to x and to ω, the volume of the parallelepiped is equal to the surface of its basis A multiplied by its height h = -ω ∧ x-, i.e.: v = A · -ω ∧ x- . By equating the two expressions for v we obtain A = -ω ∧ x- . C ORRECTION FOR E XERCISE 1.2.– (Jacobi identity) 1) We have a ∧ (b ∧ c) + 2 34 5

=Ad(a)Ad(b)·c

c ∧ (a ∧ b) 2 34 5

=−(a∧b)∧c=−Ad(a∧b)·c

Therefore, for all c

+

b ∧ (c ∧ a) 2 34 5

= 0.

=−b∧(a∧c)=−Ad(b)Ad(a)·c

Ad (a) Ad (b) · c − Ad (a ∧ b) · c − Ad (b) Ad (a) · c = 0. Or equivalently Ad (a ∧ b) = Ad (a) Ad (b) − Ad (b) Ad (a) . 2) We simply have: Ad (a ∧ b) = Ad (a) Ad (b) − Ad (b) Ad (a) = [Ad (a) , Ad (b)]. 3) We have Rb Ra R b

= = = R−1 R R = a b b = −1 R−1 R R R a b = a b =

/ 20 I/ + B · dt + 12 B2 · dt2 + ◦ dt / 00 I + A · dt + 12 A2 ·/dt2 + ◦ dt2 · Rb 0 / 0 1 2 + 12 B2 + AB · dt2 + ◦ dt2 2A /I + (A + B) · 1dt + 0 I − B · dt +/ 2 B2 · dt2 · Ra Rb 0 / 0 I/ + A · dt + 12 A2 + AB − BA · dt2 + ◦ dt2 0 −1 1 2 2 /I − A · dt + 2 A · dt2 0 · Rb/ R 0a Rb I + (AB − BA) · dt + ◦ dt2 .

−1 Thus, R−1 a Rb Ra Rb corresponds to an infinitesimal rotation following the skew symmetric matrix [A, B] = AB − BA.

As a consequence, if in a space probe, we are able to generate only two rotation motions following A and B using inertial disks, we can also generate a rotation

Three-dimensional Modeling

39

following [A, B] alternating the sequence of infinitesimal rotations with respect to B, A, −B, −A, B, A, −B, −A, . . .

4) The verification is trivial and we will not give it here. Let us note that this result enables us to deduce that the set of skew-symmetric matrices equipped with addition, bracket and the standard outer product is also a Lie algebra. C ORRECTION FOR E XERCISE 1.3.– (Varignon’s formula) The position of a point x of the solid body satisfies the state equation: x˙ = Ad (ω) · x where ω is parallel to the axis of rotation ∆ and ||ω|| is the rotation speed of the solid (in rad.s−1 ). After integrating this linear state equation, we find: x (t) = eAd(ω)t · x (0) We could also have obtained this formula by applying Rodrigues’ formula studied in exercise 1.4. This property can be interpreted by the fact that Ad (ω) represents a rotation movement, whereas its exponential represents the result of this movement. C ORRECTION FOR E XERCISE 1.4.– (Rodrigues’ formula) 1) It is sufficient to verify that: 

 0 −ωz ωy 0 −ωx  x. ω ∧ x =  ωz −ωy ωx 0

2) The solution of the state equation is: x(t) = eAt x(0). 3) At time t the solid body has turned by an angle of ||ω|| · t and therefore for t = 1, it has turned by ||ω||. Therefore, the rotation R of angle ||ω|| around ω is given by: R = eA = eω∧ .

40

Mobile Robotics

/ 0 4) The characteristic polynomial of A is s3 + ωx2 + ωy2 + ωz2 s. The eigenvalues of A are 0, i||ω||, −i||ω||. The eigenvectors associated with the eigenvalue 0 are collinear to ω. This is logical since the points of the axis of rotation have zero speed. 5) The eigenvalues of R are obtained using the eigenvalue correspondence theorem and are therefore equal to e0 , ei||ω|| , e−i||ω|| . 6) The expression of a rotation around the vector ω = (1, 0, 0) of angle α is: 

   0 0 0 1 0 0 R = exp  0 0 −α  =  0 cos α − sin α  . 0 α 0 0 sin α cos α

7) Rodrigues’ formula tells us that the orientation matrix around the vector ω of angle ϕ = ||ω|| is given by: 

 0 −ωz ωy 0 −ωx  . Rω = exp  ωz −ωy ωx 0

C ORRECTION FOR E XERCISE 1.5.– (Geometric approach to Rodrigues’ formula) 1) We have: (u) = = = = =

+ , Rn,ϕ +u|| ,+ u⊥ Rn,ϕ u|| + Rn,ϕ (u⊥ ) (by linearity of the rotation operator) u|| + (cos ϕ) u⊥ + (sin ϕ) (n ∧ u⊥ ) (see figure, on the right) < u, n > ·n + (cos ϕ) (u− < u, n > ·n) + (sin ϕ) (n∧ (u− < u, n > ·n)) < u, n > ·n + (cos ϕ) (u− < u, n > ·n) + (sin ϕ) (n ∧ u) .

Whence Rodrigues’ formula: (u) = (cos ϕ) · u + (1 − cos ϕ) (< u, n > ·n) + (sin ϕ) (n ∧ u) . 2) We have: / 0 / 0 / 0 2 n∧ (n ∧ u) = nT u · n − nT n · u = n · uT n − ||n|| u = < u, n > n − u.

Therefore: < u, n > ·n = n∧ (n ∧ u) + u.

Three-dimensional Modeling

41

Rodrigues’ formula can therefore also be written as: (u) = n∧ (n ∧ u) + u + (cos ϕ) (u − n∧ (n ∧ u) + u) + (sin ϕ) (n ∧ u) = u + (1 − cos ϕ) (n∧ (n ∧ u)) + (sin ϕ) (n ∧ u) . The operator can be represented by the following rotation matrix: / 0 Rn,ϕ = I + (1 − cos ϕ) Ad2 (n) + (sin ϕ) (Ad (n)) .

or, in a developed form:

Rn,ϕ



   −n2y − n2z nx ny nx nz 1 0 0 −n2x − n2z ny nz  =  0 1 0  + (1 − cos ϕ)  nx ny 0 0 1 nx nz ny nz −n2x − n2y   0 −nz ny 0 −nx  . + (sin ϕ)  nz −ny nx 0

3) We have

/ 0 tr (Rn,ϕ ) = 3+ (1 − cos ϕ) · −n2y − n2z − n2x − n2z − n2x − n2y = 3 − 2 (1 − cos ϕ) = 1 + 2 cos ϕ. Moreover Rn,ϕ − RTn,ϕ Thus n=



 0 −nz ny 0 −nx  . = 2 (sin ϕ)  nz −ny nx 0

/ 0 1 · Ad−1 Rn,ϕ − RTn,ϕ . 2 sin ϕ

4) The trajectory has the form R(t) = exp (tA) · Ra and we have to find A which is skew-symmetric (so that exp (tA) is a rotation matrix). For t = 0, we have R(0) = Ra and for t = 1, we want R(1) = Rb . We thus have to solve Rb = −1 exp (A) · Ra or/ equivalently 0 exp (A) = Rb · Ra , with A skew-symmetric. We may −1 write A = log Rb · Ra but the logarithm of a matrix is not unique. In this exercise, it is assumed that all matrices are of dimension 3×3. To find an interpolation trajectory

42

Mobile Robotics

matrix R(t) between Ra and Rb , we take the results of the previous question and we perform the following Rab x1 ω y1 ϕ n A

= Rb · R−1 (transformation from Ra to Rb ) a = tr (R ) − 1 (corresponds to 2cos ϕ) ab / 0 = Ad−1 Rab − RTab = -ω(corresponds to 2sin ϕ) = atan2(y1 , x1 ) ω = &ω& = ϕ · n∧ (skew matrix)

Then, we get R(t) = exp (tA) · Ra · We clearly see here that finding A such that exp (A) = Rb · R−1 a is not a unique solution. For instance, we could have taken A = (ϕ + 2kπ) n∧, with k &= 0, but several turns are now needed to go from Ra to Rb . 5) Let us recall that the Maclaurin series development of the sine and cosine functions is written as 3

5

7

sin ϕ = ϕ − ϕ3! + ϕ5! − ϕ7! + . . . 2 4 6 cos ϕ = 1 − ϕ2! + ϕ4! − ϕ6! + . . . Let H = Ad (n)./ Since 0n is an eigenvector of H associated with the zero eigenvalue, we have H n · nT = 0. Moreover / 0 H2 = n · nT − I . Therefore H3 H4 H5 H6 H7

/ 0 = H · n · nT − I = −H 2 = H · H3 = −H / 0 4 = H · H = H −H2 = −H3 = H = H · H5 = H2 = H · H6 = H3 = −H . . .

Thus, Rodrigues’ formula is written as Rn,ϕ = I + (sin ϕ) · H + (1 − cos ϕ) · H2 . - 2 . ϕ5 ϕ7 ϕ ϕ4 ϕ6 ϕ3 + − + . . . · H+ − + + . . . · H2 =I+ ϕ− 3! 5! 7! 2! 4! 6! = I + ϕ · H+

ϕ2 ϕ3 ϕ4 ϕ5 ϕ6 ϕ7 · H2 − ·H− · H2 + ·H+ · H2 − · H + ... 2! 3! 4! 5! 6! 7!

= I + ϕ · H+

ϕ2 ϕ3 ϕ4 ϕ5 ϕ6 · H2 + · H3 + · H4 + · H5 + · H6 + . . . 2! 3! 4! 5! 6!

= exp(ϕ · H).

Three-dimensional Modeling

43

Or equivalently Rn,ϕ = exp (ϕ · Ad (n)) = exp (ϕ · n∧) . C ORRECTION FOR E XERCISE 1.6.– (Quaternions) 1) We have: · 1 i j k

1 1 i j k

i i −1 −k j

j j k −1 −i

k k −j i −1

Note that the multiplication is not commutative. 2) We have 2

|˚ q| =

"

θ cos 2

#2

" #2 " #2 " #2 / 2 0 θ θ θ 2 2 + sin . v1 + v2 + v3 = cos + sin = 1. 2 2 2

3) Both quaternions ˚ q = cos( θ2 ) + sin( θ2 )· < v >, and θ+2π θ+2π ˚ q = cos( 2 ) + sin( 2 )· < v > correspond to the same rotation. Now, '

˚ q ' = cos(

θ + 2π θ + 2π θ θ ) + sin( )· < v >= − cos( ) − sin( )· < v >= −˚ q. 2 2 2 2

4) We have: (s+ < v >) · (s+ < −v >) = (s + v1 i + v2 j + v3 k) · (s − v1 i − v2 j − v3 k) = s2 − sv1 i − sv2 j − sv3 k + v1 is − v12 i2 − v1 v2 ij − v1 v3 ik + v2 js −v2 v1 ji − v22 j 2 − v2 v3 jk + v3 ks − v3 v1 ki − v3 v2 kj − v32 k 2 = s2 − sv1 i − sv2 j − sv3 k + v1 is + v12 − v1 v2 k + v1 v3 j+ v2 js + v2 v1 k + v22 − v2 v3 i + v3 ks − v3 v1 j + v3 v2 i + v32 = s2 + v12 + v22 + v32 = 1 = 1+ < 0 > . 5) (i). Since the rotations are simple, the first method is to get the result directly by hand moving a simple object for instance. We obtain a rotation with respect to (0, 1, 0) with an angle π2 .

44

Mobile Robotics

(ii) We build the rotation Euler matrix R associated to ϕ = θ = ψ = 

0 R (ϕ, θ, ψ) =  1  0 1 ·0 0

  −1 0 0 0 0· 0 0 1 −1 0 0 0 0 −1  =  0 1 0 −1

0 1 0 0 1 0

π 2.

We get

 1 0 0  1 . 0

Then, we take one normalized eigenvector v of R associated with the eigenvalue λ = 1: v = (0, 1, 0)T . The rotation R can be obtained as a rotation of an angle α around v. To compute α, we apply the formula [1.9] α = ±arccos((tr(R) − 1)) where the sign chosen to have eα·v∧ = R. We get α =

π 2.

(iii) We now use the quaternion method. We get )

√1 2

+

1 √ 2 2

√1 k 2

* ) · √12 +

√1 j 2

* ) · √12 +

√1 i 2

*

=

(1 + k) · (1 + j) · (1 + i)

1 √ 2 2

(1 + k + j + −i + i + j − k + 1) * (2 + 2j) = √12 + √12 j .

(1 + k + j + kj + i + ki + ji + kji) = 1 √ 2 2

1 √ 2 2

)

With all approaches, we obtain a rotation with an angle (0, 1, 0).

π 2

around the vector v =

C ORRECTION FOR E XERCISE 1.7.– (Schuler pendulum) ˙ α). 1) The state vector is x = (θ, α, θ, ˙ In order to have a horizontal movement, we need a horizontal force f . Following the fundamental principle of dynamics, we have: . *1 − *2 J θ¨ + α ¨ = −mg*1 sin α + mg*2 sin α + f cos α 2

/ 0 with J = m *21 + *22 and f = 2ma. Since θ¨ = ar , the state equations of the system are written as: ˙ θ    α˙ θ¨    α ¨

= θ˙ = α˙ = ar = − ar +

)2 −)1 )21 +)22

(g sin α − a cos α)

Three-dimensional Modeling

45

2) The pendulum remains vertical if, for α = 0, we have α ¨ = 0. Thus a *2 − * 1 − + 2 (g sin α − a cos α) = 0 r *1 + *22 or equivalently: −

*2 − *1 1 = . *21 + *22 r

We must therefore satisfy the equation *22 + r*2 − *1 r + *21 = 0. Solving it gives: *2 =

−r +

: r2 + 4 (*1 r − *21 ) . 2

For *1 = 1, we obtain: *2 =

−64·105 +

√

(64·105 )2 +4(64·105 −1) 2

= 1 − 3.1 × 10−7 m.

3) The equation describing the oscillations is: a 1 α ¨ = − − (g sin α − a cos α) . r r For a = 0, we have: g α ¨ = − sin α r which is the equation of a pendulum of length * = r. By linearizing this : equation, we obtain the characteristic polynomial s2 + gr and therefore the pulse ω = gr . The Schuler period is therefore equal to: T = 2π

;

r = 5072 sec = 84 min. g

4) The program is the following: √ −r+ r 2 +4()1 r−)21 ) 1 r := 10; *1 := 1; *2 := /2 2 2 dt := 0.05; x = (1, 0.1, 0, 0)T 3 For t := 0 : dt : 10 4 a := randn(1)/ 0 5 x := x + dt · 14 f (x, a) + 34 f (x + 2dt 3 f (x, a), a)

46

Mobile Robotics

Note that for an initialization α = α˙ = 0, the pendulum always points towards the center of the Earth. Otherwise, it oscillates and conserves this oscillation at the Schuler frequency. This oscillation can be observed in modern inertial units and there are methods for compensating for it using information gathered by the other inertial sensors. C ORRECTION FOR E XERCISE 1.8.– (Brake detector) 1) We have: o0 m = o0 o1 + o1 m ⇔ a0 i0 + b0 j0 = xi0 + yj0 + a1 i1 + b1 j1 . Let us express this relation in the coordinate system R0 : "

a0 b0

#

" # " # " # x cos θ − sin θ = + a1 + b1 y sin θ cos θ

which leads to: " # " # " #" # a0 x cos θ − sin θ a1 . = + b0 y sin θ cos θ b1 2 34 5 Rθ

2) We have: RTθ

˙ θ = θ˙ · ·R

"

cos θ sin θ − sin θ cos θ

#"

− sin θ − cos θ cos θ − sin θ

#

= θ˙

"

3) In the coordinate system R0 this vector u is expressed by: u|R0 =

"

a˙ 0 b˙ 0

#

=

" # " # " # x˙ ˙ θ a1 + Rθ a˙ 1 . +R b1 y˙ b˙ 1

Therefore: u|R1 = RTθ · u "|R0# " # " # x˙ T ˙ θ a1 + a˙ 1 = Rθ · + RTθ · R y˙ b1 b˙ 1 " # " # " # a˙ 1 x˙ −b1 = RTθ · + θ˙ + ˙ y˙ a1 b1 " # " # " # " # ˙ 1 + a˙ 1 a˙ 1 v −b1 v − θb = + θ˙ · + ˙ = ˙ 1 + b˙ 1 0 a1 b1 θa

0 −1 1 0

#

.

Three-dimensional Modeling

47

4) We have: " # "" # " # " ## d a ¨0 a˙ 1 x˙ a1 ˙ a|R0 = ¨ = + Rθ + Rθ ˙ y˙ b1 b0 b1 dt " # " # " # " # ¨1 x ¨ ¨ θ a1 + 2R ˙ θ a˙ 1 + Rθ a = +R ¨b1 y¨ b1 b˙ 1 Therefore: a|R1 = RTθ a|R0 " # " # " # " # ¨1 x ¨ ¨ θ a1 + 2RTθ R ˙ θ a˙ 1 + a = RTθ + RTθ R ¨b1 . y¨ b1 b˙ 1 However: " " ## d ˙ − sin θ − cos θ d ˙ Rθ = RTθ θ cos θ − sin θ dt dt " #" " # " ## cos θ sin θ − sin θ − cos θ − cos θ sin θ 2 ¨ ˙ = θ +θ − sin θ cos θ cos θ − sin θ − sin θ − cos θ " # " # " 2 # 0 −1 10 −θ˙ −θ¨ = θ¨ − θ˙2 = . 1 0 01 θ¨ −θ˙2

¨ θ = RTθ RTθ R

We get a|R1 = =

"

"

α β

#

+

"

−θ˙2 −θ¨ θ¨ −θ˙2

#"

a1 b1

#

¨ 1 − 2θ˙b˙ 1 + a α − θ˙2 a1 − θb ¨1 2 ¨ ˙ ˙ β + θa1 − θ b1 + 2θa˙ 1 + ¨b1

+ 2θ˙ #

"

−b˙ 1 a˙ 1

#

+

"

.

5) The vehicle in front of us is braking if < a|R1 , u|R1 > ≤ 0, i.e. if "

¨ 1 − 2θ˙b˙ 1 + a α − θ˙2 a1 − θb ¨1 2 ¨ ˙ ˙ β + θa1 − θ b1 + 2θa˙ 1 + ¨b1

#T " # ˙ 1 + a˙ 1 v − θb ≤ 0. · ˙ 1 + b˙ 1 θa

a ¨1 ¨b1

#

48

Mobile Robotics

C ORRECTION FOR E XERCISE 1.9.– (Modeling an underwater robot) The derivative of the position vector is obtained by noticing that:

p˙ = vi1

 cos θ cos ψ = v  cos θ sin ψ  − sin θ

[1.9]



since i1 corresponds to the first column of matrix [1.9]. Finally, by taking into account equations [1.12], we can write the state equations for our submarine:  p˙ x     p˙ y     p˙ z  v˙   ϕ˙     θ˙    ˙ ψ

= v cos θ cos ψ = v cos θ sin ψ = −v sin θ = u1 = u4 + tan θ · (sin ϕ · vu2 + cos ϕ · vu3 ) = cos ϕ · vu2 − sin ϕ · vu3 ϕ cos ϕ = sin cos θ · vu2 + cos θ · vu3

Here we have a kinematic model (i.e. it does not involve forces or torques). There are no parameters and the model can therefore be considered correct if the underwater robot is solid (that is, it cannot be contorted), and if its trajectory is tangent to the axis of the robot. Such a model allows us to use nonlinear control methods such as feedback linearization which will be discussed in Chapter 2. These methods are indeed less robust with respect to a model error, but are formidably efficient if an accurate model for our system is known. C ORRECTION FOR E XERCISE 1.10.– (3D robot graphics) 2) To draw the robot at state x matrix  0 0 10 0 0  −1 1 0 −1 −0.2  M= 0 0 0 0 1 1 1 1 1 1 pattern matrix (to be painted) 

 px  R (ϕ, θ, ψ) py   ·M  pz  00 0 1

= (px , py , pz , v, ϕ, θ, ψ), we build the pattern  10 0 0 0 0.2 1   and we compute the transformed 0 1 0 1 1 1

Three-dimensional Modeling

49

3) The simulation could be done by using Euler’s integration method as shown by Figure 1.19 with the initial vector x (0) = (−5, −5, 12, 15, 0, 1, 0)T T

and the control u = (0, 0, 0.2) . This simulation will be picked up in exercise 2.4 for performing the control of the trajectory of the robot.

Figure 1.19. Simulation of the underwater robot. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

C ORRECTION FOR E XERCISE 1.11.– (Manipulator robot) One must draw the arms one after the other. For this, we have to build the translation by a vector v and the rotation around ω with an angle -ω-. They are represented by the two matrices     1 0 0 vx 0  0 1 0 vy   ω∧ 0    and Rω =  e Tv =   0 0 1 vz   0  000 1 00 0 1

50

Mobile Robotics

In this exercise, we need a translation of length r following the z-axis, a translation of length d following x, a rotation of α around y and a rotation of θ. They are respectively given by the 4 × 4 matrices T 0  , T d  , R 0  and R 0    0

  0

r

  α

0

  0

0

θ

The seven arms of the robot in a configuration q (whose components are the coordinates of the articulations) can be drawn as follows: R0 = # Rj = Ri · T

0



   0 

rj

Id4 · T dj  · R

#

   0 

0

Rj · R

Rj+1 =

0



0



   aj 

0

   0 

qj

- # . Each arm is drawn using the pair of two homogeneous matrices Rj , Rj , for j ∈ {1, . . . , 7}. Figure 1.20 corresponds to a simulation for a robot with the following parameter vectors: 

0

 π  2  0  π a=  − π2 −  π2 − 2 0



    ,    



   0 1  0.5   0.5       0   1         d=  −0.1  , r =  0.1  .  −0.3   1       −1   0.2  0 0.2

C ORRECTION FOR E XERCISE 1.12.– (Floating wheel) 1) Considering the Euler’s rotation equation Iω˙ r + ω r ∧ (Iω r ) = τ r where the torque τ r = 0, and the fact that we have no acceleration of our wheel, from [1.12] we get

Three-dimensional Modeling

    p˙    ϕ˙    ˙  θ ˙  ψ     v˙ r   ω˙ r

= R  (ϕ, θ, ψ) · vr  1 tan θ sin ϕ tan θ cos ϕ − sin ϕ  · ω r =  0 cos ϕ sin ϕ cos ϕ 0 cos θ cos θ = −ω r ∧ vr = −I−1 · (ω r ∧ (I · ω r ))

51

(i) (ii)

[1.16]

(iii) (iv)

Figure 1.20. Simulation the manipulator robot

2) For the simulation, we take x = (p, ϕ, θ, ψ, vr , ω r ) = (0, 0, 2 , 0, 0, 0 , 5, 0, 0 , 5, 1, 0) 2 34 5 2 34 5 2 34 5 2 34 5 p

ϕ,θ,ψ

vr

ωr

and we get the result depicted in Figure 1.12. The wheel translates with respect to px . We see from the shadow (black) that the rotation axis oscillates. This corresponds to the precession.

52

Mobile Robotics

Figure 1.21. Motion of the wheel with a torque-free precession. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

3) We have L = RIRT ω = RIω r . Therefore ˙ r + R ·I· ˙ L˙ = RIω 2345ω r + R · I · =0

= = =

˙ r − R (ω r ∧ (I · ω r )) RIω ˙ r − RRT RI ˙ · ωr RIω ˙ r − RI ˙ · ω r = 0. RIω

ω˙ 2 34r 5

=−I−1 ·(ω

r ∧(I·ω r ))

Three-dimensional Modeling

53

Moreover E˙ K = =

=

/

0

d 1 T dt 2 L Rω r 1 ˙T L · Rω r + 12 2345 LT 2 2345 =0 =ω Tr IRT

T ˙ 1 T R· 2 (ω r IR 2 34 =0

ωr + 5

. ˙ r + Rω˙ r · Rω 4

ω Tr IRT R(−I−1

= − 12 ω Tr (ω r ∧ (I · ω r )) = 0.

˙r =ω

52 3 · (ω r ∧ (I · ω r )))

See [1.16,iv]

4) We write the following P YTHON code using the S YMPY library: from sympy import * x = MatrixSymbol(’x’, 12, 1) wx,wy,wz,rho,m,s=symbols("wx wy wz rho m s") phi,theta,psi=x[3:6] vr=Matrix(x[6:9]) wrx,wry,wrz=x[9:12] wr=Matrix(x[9:12]) ci,si,cj,sj,ck,sk=cos(phi),sin(phi),cos(theta),sin(theta), cos(psi),sin(psi) Ri = Matrix([[1,0,0],[0,ci,-si],[0,si,ci]]) Rj = Matrix([[cj,0,sj],[0,1,0],[-sj,0,cj]]) Rk = Matrix([[ck,-sk,0],[sk,ck,0],[0,0,1]]) E= Rk*Rj*Ri dp=E*vr da= Matrix([[1,si*sj/cj,ci*sj/cj],[0, ci,-si], [0,si/cj,ci/cj]])*wr A=Matrix([[0,-wrz,wry],[wrz,0,-wrx],[-wry,wrx,0]]) dvr=-A*vr I=(m/2)*rho**2*Matrix([[1,0,0],[0,1/2,0],[0,0,1/2]]) dwr=-(I**-1)*(A*I*wr) f=Matrix((dp,da,dvr,dwr)) J=f.jacobian(x) x0=Matrix([[0],[0],[0],[0],[0],[0],[0],[0],[0],[wx],[wy],[wz]]) J=J.subs(x,x0) The matrix J at point x0 is written in Figure 1.23. The yellow blocks with zeros can be understood by the graph of Figure 1.23. An arc represents a differential delay. For instance, the arc between the node vr and p means that p˙ depends algebraically on vr .

54

Mobile Robotics

Figure 1.22. Jacobian matrix of the evolution function at point x0 . For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Figure 1.23. Graph of the differential delays for the floating wheel. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

The matrix J is block triangular and we can easily compute the characteristic polynomial. It is given by P (s) = s6 · (s2 + ωx2 ) · (s2 + ωz2 ) · (s2 + ωx2 + ωy2 + ωz2 ). The term (s2 +ωx2 +ωy2 +ωz2 ) is consistent with the fact when no precession exists, the wheel spins around ω r at a pulsation -ω r -. The terms (s2 + ωx2 ) and (s2 + ωz2 ) correspond to the precession.

Three-dimensional Modeling

55

If the wheel is not perfectly solid, internal frictions will tend to dampen the precession, and the rotation axis will align itself with one eigenvector of I. This vector can either be one vector of the wheel plane, or the axis of the wheel (orthogonal to the wheel plane). C ORRECTION FOR E XERCISE 1.13.– (Schuler oscillations in an inertial unit) 1) Since the Earth is static, we have   g  0  and ω r = 0. ames = r 0

2) All Euler angles of R2 are constant (as for R1 ) and equal to zero. They do not appear as state variables anymore. The Euler matrix R (ϕ, θ, ψ) is equal to the identity. The state equations of R2 become p˙ v˙ r

= =

vr g (p) 2 34 5



  =−g  

√ x x2 +z 2

0

√ z x2 +z 2

+ ames − ω r ∧ vr 2 34 5 2 r34 5    =0 g       = 0    0

They can be rewritten as follows:  x˙ = vx  .    v˙ = g 1 − √ x x x2 +z 2  z ˙ = v z    v˙ = −g √x2z+z2 z

3) We get the trajectory represented in Figure 1.24. We observe some oscillations known as the Schuler oscillations. 4) Around z = 0, x = r, we get      0 1 0 0 x−r x˙  v˙ x   0 0 0 0   vx    =   z˙   0 0 0 1   z  . 0 0 − gr 0 vz v˙ z

/ : : 0 The eigenvalues are 0, 0, j gr , −j gr . Some oscillations exist and this system is unstable due to the fact that we have two roots in 0.

5) In practice, the inertial unit is not perfectly initialized, and thus it finds a trajectory similar to that of R2 whereas it is fixed for R1 . Since the error is small, the

56

Mobile Robotics

linear approximation is realistic. The integration method inside the inertial unit returns some unwanted oscillations corresponding to a solution which is not the actual one, < as illustrated by Figure 1.25. These oscillations correspond to a period T = 2π gr = 5072 sec = 84 min, named the Schuler period. For many applications (such as when we are in an aircraft), we know that such oscillations are dummies and we may improve the integration method in order to damp these Schuler oscillations.

Figure 1.24. The robot R2 with the trajectory painted blue and the robot R1 fixed in o1 both undergo the same rotation rate and the same acceleration

C ORRECTION FOR E XERCISE 1.14.– (Lie bracket for control) 1) We have dg df [f , g] (x) = dx · f (x) − dx · g (x) = B·A·x−A·B·x = (BA − AB) · x.

2) Without loss of generality, the proof will be given for t = 0. We shall use the following notations df dx (x(t)) dg dx (x(t))

↔ At ↔ Bt

Three-dimensional Modeling

57

Figure 1.25. Fake periodic trajectory returned by the inertial unit which feels the same as if it were static as for R1 . The corresponding measured acceleration ames is painted r red. The right subpicture corresponds to a zoom around R1 . For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

For a given t, and a small δ we have ˙ ¨ (t) · x(t + δ) − x (t) = x(t) ·δ+x with

/ 0 δ2 + o δ2 2

[1.17]

˙ x(t) = f (x (t)) u1 + g (x (t)) u2 ¨ (t) = (At · u1 + Bt · u2 ) x(t) ˙ x (if u is constant) Thus ˙ x(0) ¨ (0) x ˙ x(δ) ¨ (δ) x

= = = =

−f (x (0)) ˙ −A0 x(0) = A0 f (x (0)) −g (x (δ)) ˙ −Bδ x(δ) = Bδ g (x (δ))

(u1 = −1, u2 = 0) (u1 = 0, u2 = −1)

We get x(δ) − x (0)

[1.17]

=

[1.17]

−f (x(0))δ + A0 · f (x(0)) ·

x(2δ) − x (δ) = −g(x(δ))δ + Bδ · g(x(δ)) · The sum yields 2

δ2 2 δ2 2

/ 0 + o δ2 / 0 + o δ2

x(2δ) − x (0) = −f (x(0)) · δ + A0 · f (x(0)) · δ2 − g(x(δ)) · δ / 0 2 + Bδ · g(x(δ)) · δ2 + o δ 2 .

[1.18]

58

Mobile Robotics

Now, ˙ (i) g(x(δ)) = g(x(0)) + B0 . x(0) 2345

· δ + o (δ) = g(x(0)) + o (1)

=−f (x(0))

(ii)

Bδ = B0 + o(1)

Therefore x(2δ) − x (0) = −f (x(0)) · δ + A0 · f (x(0)) · − (i)

g(x(δ)) 2 34 5

= g(x(0))−B0 .f (x(0))·δ+o(δ)

·δ+ (ii)

Bδ 2345

· (i)

= B0 +o(1) 2

δ = −f (x(0)) · δ + A0 · f (x(0)) · 2

δ2 2 g(x(δ)) 2 34 5

·

δ2 2

= g(x(0))+o(1)

−g(x(0)) · δ + B0 · f (x(0)) · δ 2 + B0 · g(x(0)) ·

/ 0 δ2 + o δ2 2

/ 0 + o δ2

[1.19]

The same reasoning yields 2

x(−2δ) − x (0) = −g(x(0)) · δ + B0 · g(x(0)) · δ2 −f (x(0)) · δ + A0 .g(x(0)) · δ 2 + A0 · f (x(0)) ·

δ2 2

/ 0 + o δ2

This result could be obtained directly from [1.19] by rewriting: δ → −δ, f → −g, g → −f , A0 → −B0 , B0 → −A0 . Thus / 0 x(2δ) − x(−2δ) = (B0 · f (x(0)) − A0 · g(x(0)))δ 2 + o δ 2 2 34 5 [f ,g](x(0))

The consequence is that, using the periodic sequence, we are now able to move with respect to the direction [f , g]. 3) We have shown that within a time period of 4δ we moved in the direction [f , g] δ2 by [f , g]δ 2 . This means that we follow the field 4δ [f , g] = 4δ [f , g] which is infinitesimal. Multiplying the cyclic sequence by the scalar α ∈ R amounts to multiplying both f , g by α. The field thus becomes 4δ [αf , αg] = 4δ α2 [f , g]. If we < want the field ν · [f , g], we have to take multiply the sequence by α = 4|ν| δ . If ν is negative, we have to change the orientation of the sequence. The cyclic sequence is thus {(εα, 0) , (0, α) , (−εα, 0) , (0, −α)}

Three-dimensional Modeling

59

where ε = sign(ν) changes the orientation of the sequence (clockwise for ε = 1, counterclockwise for ε = −1). 4) If we want to follow the field a1 f + a2 g + a3 [f , g], we have to take the sequence {(a1 + εα, a2 ) , (a1 , a2 + α) , (a1 − εα, a2 ) , (a1 , a2 − α)} with α =


dim y. In this case, the matrix A (x) is rectangular. In order to apply the transformation in [2.8], we may use a Moore-Penrose pseudoinverse. If A is of full rank (i.e. its rank is equal to dim y), this pseudoinverse is given by: / 0−1 A† = AT · A · AT .

We will therefore have:

dim v = dim y < dim u and we are in a situation identical to that if the square robot (i.e. non-redundant). 2.3.2. Relative degree By properly analyzing the path used to obtain equation [2.7], we realize that the (k) k th derivative of the ith output yi is expressed in the form: (k)

= ˆbik (x) if j < ki

(k)

=ˆ aTik (x) · u + ˆbik (x) if k = ki

(k)

=ˆ aik (x, u, u, ˙ u ¨ , . . . ) if k > ki

yi yi

yi

The coefficient ki is called the relative degree of the ith output. By measuring the state of the system x and its input u, we can thus have all the successive derivatives (k) of the outputs yi , as long as k remains smaller than or equal to ki . Indeed, given the high-frequency noise that appears in the signals, we cannot reliably obtain the derivative of the signals by using derivators. We, therefore, have an analytic function: ∆:

Rm → R(k1 +1)·····(km +1) (k ) (k ) (x, u) 0→ ∆(x, u) = (y1 , y˙ 1 , . . . , y1 1 , y2 , y˙ 2 , . . . , ym m )

that allows us to have all the derivatives of the outputs (until their relative degree), and this without using digital derivators.

72

Mobile Robotics

E XAMPLE.– Let us consider the system described by: !

x˙ = xu + x3 y = 2x.

We have: y = 2x y˙ = 2x˙ = 2xu + 2x3 y¨ = 2xu ˙ + 2xu˙ + 6xx ˙ 2 = 2(xu + x3 )u + 2xu˙ + 6(xu + x3 )x2 . We, therefore, have a relative degree k = 1 for the output y. We can therefore have y˙ without using digital derivators. This is not the case for y¨ because having u with a high level of precision does not mean that we have u. ˙ Here, we have ∆(x, u) = (2x, 2xu + 2x3 ). 2.3.3. Differential delay matrix We call differential delay rij separating the input uj from the output yi the number of times we need to differentiate yi in order to make uj appear. The matrix R of the rij is called the differential delay matrix. When plainly reading the state equations, this matrix can be obtained without calculation, simply by counting the number of integrators each input uj must be subjected to in order to algebraically affect the output yi (some examples are discussed in more detail in the exercises). The relative degree for each output can be obtained by taking the minimum of each row. Let us take for example: 

 1 2 2 R = 3 4 3. 4 ∞ 2

The associated system is composed of three inputs, three outputs and the relative degrees (components in bold in the preceding formula) are k1 = 1, k2 = 3, k3 = 2. If there is a j such that ∀i, rij > ki (or equivalently if a column has no element in bold), the matrix R is called unbalanced. In our example, it is unbalanced since there is a j (k ) (here j = 2) such that ∀i, rij > ki . If the matrix is unbalanced, then for all i, yi i th does not depend on uj . In this case, the j column of A(x) will be zero and A(x) will always be singular. Thus, the transformation [2.8] will have no meaning. One method to overcome this is to delay some of the inputs uj by adding one or more integrators in front of the system. Adding an integrator in front of the j th input amounts to adding

Feedback Linearization

73

1 to the j th column of R. In our example, if we add an integrator in front of u1 , we obtain:   2 2 2 R = 4 4 3. 5 ∞ 2

The relative degrees become k1 = 2, k2 = 3, k3 = 2 and the matrix R becomes balanced. 2.3.4. Singularities The matrix A(x) involved in feedback [2.8] may not be invertible. The values for x such that det (A (x)) = 0 are called singularities. Although they generally form a set of zero measures in the state space, studying singularities is fundamental since they are sometimes impossible to avoid. We call set of acceptable outputs of the system [2.6] the quantity: Sy = {y ∈ Rm | ∃x ∈ Rn , ∃u ∈ Rm , f (x) + g(x) · u = 0, y = h(x)} . The set Sy is therefore composed, by the projection on Rm , of a differentiable manifold (or surface) of dimension m (since we have m + n equations for 2m + n variables). Thus, except for the degenerate case, Sy is a subset of Rm with a non-empty interior and an exterior. In order to properly understand this, consider the example of a cart on rails as represented on Figure 2.3. This cart can be propelled by a horizontal ventilator whose angle of thrust can be controlled. But be careful, the rotation speed of the ventilator is fixed.

Figure 2.3. Robot cart propelled by a ventilator

The state equations that model this system are given by:   x˙ 1 = u x˙ 2 = cos x1 − x2  y = x2

74

Mobile Robotics

where x1 is the ventilator’s angle of thrust, x2 is the speed of the cart. Let us note that we have taken into account a viscous friction force. The set of acceptable outputs is: Sy = {y | ∃x1 , ∃x2 , ∃u, u = 0, cos x1 − x2 = 0, y = x2 } = {y | ∃x1 , cos x1 = y} = [−1, 1]. This means that we will not be able to stabilize the cart at a speed that is strictly superior to 1, in absolute value. Let us apply a feedback linearizing method. We have: y˙ = cos x1 − x2 y¨ = − (sin x1 ) u − cos x1 + x2 and therefore, the linearizing controller is given by: u=

−1 (v + cos x1 − x2 ) . sin x1

The feedback system therefore has the following equation: y¨ = v. It may appear that any value for y can be reached, since v can be chosen arbitrarily. This would be correct, if we would not have the singularity / 0 that appears when sin x1 = 0. Let us take, for example, v (t) = 1 and x(0) = π3 , 0 . We should have: y¨(t) = v (t) = 1 = t 1 y(t) ˙ = y˙ (0) + y¨(τ )dτ = cos x1 (0) − x2 (0) + t = + t 2 0 = t 1 1 y(t) = y (0) + y(τ ˙ )dτ = x2 (0) + t2 + t = t2 + t 2 2 0

which is physically impossible. This is what happens when we apply such a controller: the input u directs the angle of the ventilator towards the correct direction, and the equation y¨ = v is then satisfied – at least in the very beginning. Then x1 is canceled out and the singularity is reached. The equation y¨ = v can no longer be satisfied. For some systems, it can happen that such a singularity can be crossed. This is not the case here.

Feedback Linearization

75

2.4. Cart 2.4.1. First model Consider a cart described by the following state equations:  x˙ = v cos θ    y˙ = v sin θ  θ˙ = u1   v˙ = u2

where v is the speed of the cart, θ its orientation and (x, y) the coordinates of its center (see Figure 2.4).

Figure 2.4. Cart (also called Dubin’s car)

The state vector is given by x = (x, y, θ, v). We would like to calculate a controller that would allow us to describe a cycloid with the equation: !

xd (t) = R sin(f1 t) + R sin(f2 t) yd (t) = R cos(f1 t) + R cos(f2 t)

with R = 15, f1 = 0.02 and f2 = 0.12. For this, we use a feedback linearizing method. We have: " # " # " #" # x ¨ u2 cos θ − u1 v sin θ −v sin θ cos θ u1 = = . y¨ u2 sin θ + u1 v cos θ v cos θ sin θ u2 If we take as input: "

u1 u2

#

=

"

−v sin θ cos θ v cos θ sin θ

#−1 "

v1 v2

#

76

Mobile Robotics

where (v1 , v2 ) is the new input vector, we obtain the linear system: " # " # x ¨ v1 = . y¨ v2 Let us transform this system so that all our poles are at −1. Following [2.2], we obtain: ! v1 = (xd − x) + 2 (x˙ d − x) ˙ +x ¨d = (xd − x) + 2 (x˙ d − v cos θ) + x ¨d v2 = (yd − y) + 2 (y˙ d − y) ˙ + y¨d = (yd − y) + 2 (y˙ d − v sin θ) + y¨d The transformed system then obeys the following differential equations: "

(xd − x) + 2 (x˙ d − x) ˙ + (¨ xd − x ¨) (yd − y) + 2 (y˙ d − y) ˙ + (¨ yd − y¨)

#

" # 0 = . 0

If we define the error vector e = (ex , ey ) = (xd − x, yd − y), the error dynamics are written as: " # " # ex + 2e˙ x + e¨x 0 = , ey + 2e˙ y + e¨y 0 which is stable and quickly converges towards 0. The controller will therefore be: #−1 " # " # " −v sin θ cos θ (xd − x) + 2 (x˙ d − v cos θ) + x ¨d u1 = [2.9] v cos θ sin θ (yd − y) + 2 (y˙ d − v sin θ) + y¨d u2 with: x˙ d (t) y˙ d (t) x ¨d (t) y¨d (t)

= Rf1 cos(f1 t) + Rf2 cos(f2 t) = −Rf1 sin(f1 t) − Rf2 sin(f2 t) = −Rf12 sin(f1 t) − Rf22 sin(f2 t) = −Rf12 cos(f1 t) − Rf22 cos(f2 t).

2.4.2. Second model Let us now assume that the cart is described by the state equations:   x˙ = u1 cos θ y˙ = u1 sin θ ˙ θ = u2 .

Let us choose as output the vector y = (x, y). The method of feedback linearization leads to a matrix A(x) which is still singular. As it was explained in

Feedback Linearization

77

section 2.3.3, this can be predicted without any calculation simply by observing the differential delay matrix: R=

"

1 2 1 2

#

.

This matrix contains a column whose elements never correspond to the minimum of the related row (i.e. a column without elements in bold). We will illustrate how to get out of such a situation by adding integrators in front of certain inputs. Let us for instance add an integrator, whose state variable will be denoted by z, in front of the first input. Recall that adding an integrator in front of the j th input of the system means delaying this input and therefore adding 1 to all the elements of column j of R. The matrix R is then in equilibrium. We obtain a new system described by  x˙ = z cos θ    y˙ = z sin θ θ˙ = u2    z˙ = c1 .

We have !

x ¨ = z˙ cos θ − z θ˙ sin θ = c1 cos θ − zu2 sin θ y¨ = z˙ sin θ + z θ˙ cos θ = c1 sin θ + zu2 cos θ

i.e. " # " #" # x ¨ cos θ −z sin θ c1 = . y¨ sin θ z cos θ u2 The matrix is not singular, except in the unlikely case where the variable z is zero (here, z can be understood as the speed of the vehicle). The method of feedback linearization can therefore work. Let us take: "

c1 u2

#

=

"

cos θ −z sin θ sin θ z cos θ

#−1 "

v1 v2

#

=

"

cos θ sin θ − sinz θ cosz θ

#"

v1 v2

#

in order to have a feedback system of the form: " # " # x ¨ v1 = . y¨ v2 Figure 2.5 illustrates the feedback linearization that we have just performed.

78

Mobile Robotics

Figure 2.5. Dynamic feedback linearization

In order to have all the poles at −1, we need to take (see equation 2.2): " # " #" # cos θ sin θ c1 (xd − x) + 2 (x˙ d − z cos θ) + x ¨d = u2 (yd − y) + 2 (y˙ d − z sin θ) + y¨d − sinz θ cosz θ The state equations of the controller are therefore:  ¨d )   z˙ = (cos θ) (xd − x + 2 (x˙ d − z cos θ) + x   + (sin θ) (yd − y + 2 (y˙ d − z sin θ) + y¨d )  u1 = z  sin θ  ˙ d − z cos θ) + x ¨d )  z (xd − x + 2 (x  u2 = − cos  + z θ · (yd − y + 2 (y˙ d − z sin θ) + y¨d )

2.5. Controlling a tricycle

2.5.1. Speed and heading control Let us consider the tricycle represented in Figure 2.6. Its evolution equation is given by:     x˙ v cos δ cos θ  y˙   v cos δ sin θ       θ˙  =  v sin δ       v˙    u1 ˙δ u2

Feedback Linearization

79

Figure 2.6. Tricycle robot to be controlled

We have assumed here that the distance between the center of the rear axle and the axis of the front wheel was equal to 1 m. Let us choose as output the vector y = (v, θ) . The first-order derivatives of the outputs y1 and y2 are expressed by: y˙ 1 = v˙ = u1 , y˙ 2 = θ˙ = v sin δ. Since the derivative y˙ 2 of y2 does not involve the input, we may differentiate it once more: y¨2 = v˙ sin δ + v δ˙ cos δ = u1 sin δ + u2 v cos δ. The expressions for y˙ 1 and y¨2 can be rewritten in matrix form: "

y˙ 1 y¨2

#

"

#" # 1 0 u1 = . sin δ v cos δ u2 2 34 5 A(x)

By setting the feedback u = A−1 (x) v, where v is the new input, our feedback system is rewritten as: SL :

"

y˙ 1 y¨2

#

=

"

v1 v2

#

and, therefore, becomes linear and decoupled. We now have two decoupled monovariate systems. The first, of order 1, can be stabilized by a proportional controller. As for the second, second-order system, a proportional-derivative

80

Mobile Robotics

controller is best adapted. If w = (w1 , w2 ) represents the setpoint for y, this controller is expressed by: ! v1 = (w1 − y1 ) + w˙ 1 v2 = (w2 − y2 ) + 2 (w˙ 2 − y˙ 2 ) + w ¨2 if we want all our poles to be equal to −1 (see equation [2.2]). Therefore the equations of a state feedback controller for our nonlinear system are given by: " #−1 " # (w1/− v) + w˙ 1 0 1 0 u= [2.10] δ sin δ v cos δ w2 − θ + 2 w˙ 2 − v sin +w ¨2 L

Let us note that this controller does not have a state variable. It is therefore a static controller. N OTE.– Since: det (A (x)) =

v cos δ L

can be zero, there are singularities for which the control u is not defined. Appropriate processing has to be provided when such singularities are encountered by the system. 2.5.2. Position control Let us now try to make our tricycle follow a desired trajectory (xd , yd ). For this, let us choose as output the vector y = (x, y) . We have:  x˙ = v cos δ cos θ    x ¨ = v˙ cos δ cos θ − v δ˙ sin δ cos θ − v θ˙ cos δ sin θ    = u1 cos δ cos θ − vu2 sin δ cos θ − v 2 sin δ cos δ sin θ y˙ = v cos δ sin θ      y¨ = v˙ cos δ sin θ − v δ˙ sin δ sin θ + v θ˙ cos δ cos θ   u1 cos δ sin θ − vu2 sin δ sin θ + v 2 sin δ cos δ cos θ

Thus: " # " #" # " 2 # x ¨ cos δ cos θ −v sin δ cos θ u1 −v sin δ cos δ sin θ = + . y¨ cos δ sin θ −v sin δ sin θ u2 v 2 sin δ cos δ cos θ 2 34 5 2 34 5 A(x)

b(x)

However, the determinant of A(x) is zero since the two columns of the matrix A(x) are collinear to the vector (cos θ, sin θ). This means that the controllable part of the acceleration is forcibly in the heading direction of the vehicle. Thus, x ¨ and y¨ will not be independently controllable. The method of feedback linearization can, therefore, not be applied.

Feedback Linearization

81

2.5.3. Choosing another output In order to avoid having a singular matrix A(x), let us now choose the center of the front wheel as output. We have: y=

"

x + cos θ y + sin θ

#

.

By differentiating once, we have: "

y˙ 1 y˙ 2

#

=

"

x˙ − θ˙ sin θ y˙ + θ˙ cos θ

#

=v

"

cos δ cos θ − sin δ sin θ cos δ sin θ + sin δ cos θ

#

=v

"

cos (δ + θ) sin (δ + θ)

#

Differentiating again, we obtain: "

y¨1 y¨2

#

>

. ? v˙ cos (δ + θ) − v δ˙ + θ˙ sin (δ + θ) = v˙ sin (δ + θ) + v cos (δ + θ) " # u1 cos (δ + θ) − v (u2 + v sin δ) sin (δ + θ) = u1 sin (δ + θ) + v (u2 + v sin δ) cos (δ + θ)

And therefore " # " #" # " # y¨1 cos (δ + θ) −v sin (δ + θ) u1 − sin (δ + θ) = + v 2 sin δ y¨2 sin (δ + θ) v cos (δ + θ) u2 cos (δ + θ) 2 34 5 2 34 5 A(x)

b(x)

The determinant of A(x) is never equal to zero, except when v = 0. The linearizing control is therefore u = A−1 (x) · (v − b(x)) . And thus the tricycle control (the one that places all the poles at −1) is expressed by: # " ## xd x + cos θ − yd y + sin θ "" # " ## " # # x˙ d v cos (δ + θ) x ¨d +2 − − b(x) + y˙ d v sin (δ + θ) y¨d

u = A−1 (x)

"""

where w = (xd , yd ) is the desired trajectory for the output y.

82

Mobile Robotics

2.6. Sailboat Automatic control for sailing robots [ROM 12] is a complex problem, given the strong nonlinearities implied in the evolution of the system. Here we will consider the sailboat of Figure 2.7 whose state equations [JAU 05] are given by:  x˙ = v cos θ     y˙ = v sin θ − 1     θ˙ = ω     δ˙s = u1  [2.11] δ˙r = u2    v˙ = fs sin δs − fr sin δr − v     ω˙ = (1 − cos δs ) fs − cos δr · fr − ω     f = cos (θ + δs ) − v sin δs   s fr = v sin δr

Figure 2.7. Sailing robot to be controlled

This is of course a normalized model in which many coefficients (masses, lengths, etc.) have been set to 1 in order to simplify the following developments. The state vector x = (x, y, θ, δs , δr , v, ω), of dimension 7, is composed of: – position coordinates, i.e. the x, y coordinates of the sailboat’s center of gravity, the orientation θ, and the angles δs and δr of the sail and the rudder; – kinematic coordinates v and ω representing respectively the speed of the center of gravity and the angular speed of the boat. The inputs u1 and u2 of the system are the differentials of the angles δs and δr . The letters “s” and “r” refer respectively to the sail and the rudder.

Feedback Linearization

83

2.6.1. Polar curve Let us take as outputs y = (θ, v). The polar curve is the set of acceptable outputs (refer to section 2.3.4), i.e. the set Sy of all pairs (θ, v) over which we are able to stabilize. In stationary regime, we have: θ˙ = 0, δ˙s = 0, δ˙r = 0, v˙ = 0, ω˙ = 0 Thus, following the state equations in [2.11], we obtain: Sy = {(θ, v) | fs sin δs − fr sin δr − v = 0 (1 − cos δs ) fs − cos δr fr = 0 fs = cos (θ + δs ) − v sin δs fr = v sin δr } An interval calculation method [HER 10] allows us to obtain the estimation of Figure 2.8.

Figure 2.8. Internal frame (in light grey) and external frame (in dark grey) of the polar curve

2.6.2. Differential delay We may associate with the state equations of our sailboat a differential delay graph between the variables (see Figure 2.9). Within this graph, a solid arrow can be interpreted – depending on the reader – either as a cause and effect relationship, a differential delay, or a state equation. A dotted arrow represents an algebraic (and not a differential) dependency. On the graph, we can distinguish two types of variables:

84

Mobile Robotics

the state variables, pointed at by solid arrows and link variables (in grey), pointed at by dotted arrows. The derivative of a state variable is expressed as an algebraic function of all the variables which are directly before it. Likewise, a link variable is an algebraic function of the variables which are directly before it. The differential delay between a variable and an input uj is thus the minimum number of solid arrows to traverse in order to reach this variable from uj . Just as in [JAU 05], let us take as output the vector y = (δs , θ). The differential delay matrix is: R=

"

1 ∞ 3 3

#

.

The infinity here can be interpreted as the fact that there is no causal connection that links u2 to δs . The relative degrees are therefore k1 = 1 and k2 = 3.

Figure 2.9. Graph of the differential delays for the sailing robot

2.6.3. The method of feedback linearization Let us recall that the outputs (these are actually setpoint variables) chosen are the sail opening y1 = δs and the heading y2 = θ. In order to apply a feedback linearization method, we first of all need to differentiate the outputs as many times as the relative degree requires it, i.e. three times for θ and once for δs . By ... looking at the differential dependency graph, we can observe that in order to express θ in function of x and u, we need to do the same with ω ¨ , ω, ˙ δ˙r , δ˙s , f˙r , f˙s , v. ˙ This yields:  v˙     f˙s    ˙ fr ω˙      ¨ ω ...  θ

= = = = = =

fs sin δs − fr sin δr − v − (ω + u1 ) sin (θ + δs ) − v˙ sin δs − vu1 cos δs v˙ sin δr + vu2 cos δr (1 − cos δs ) · fs − cos δr · fr − ω u1 sin δs · fs + (1 − cos δs ) · f˙s + u2 sin δr · fr − cos δr · f˙r − ω˙ ω ¨

Feedback Linearization

85

We have: " # " # " #" # ˙s y ˙ 1 0 u1 δ 1 ... ... y 2 = θ = fs sin δs fr sin δr u2 2 34 5 A1 (x)

"

#" # " # 0 0 f˙s 0 + + 1 − cos δs − cos δr −ω˙ f˙r 2 34 5 2 34 5 A2 (x)

b1 (x)

However: " # " #" # f˙s − (sin (θ + δs ) + v cos δs ) 0 u1 = 0 v cos δr u2 f˙r 2 34 5 A3 (x)

"

# −ω sin (θ + δs ) + v˙ sin δs + v˙ sin δr 2 34 5 b2 (x)

And thus we have a relation of the form: " # y ˙ 1 ... = A1 u + A2 (A3 u + b2 ) + b1 y 2

= (A1 + A2 A3 ) u + A2 b2 + b1 = Au + b ... In order to set (y˙ 1 , y 2 ) to a certain setpoint v = (v1 , v2 ) , we need to take: u = A−1 (x) (v − b (x)) The system looped in this manner is governed by the differential equations: ! y˙ 1 = v1 SL : ... [2.12] y 2 = v2 which are linear and decoupled. The linearized system is of order 4 instead of 7. We have thus lost control over three variables which happen to be x, y and v. The loss of control over x and y was predictable (we want the boat to advance and, therefore, it is only natural that this corresponds to an instability for these two variables x and y). As for the loss of control over v, this is without consequence since the associated dynamics are stable. How indeed would it be possible to design a boat that would be able to keep a fixed heading and sail opening, without its speed converging towards a finite value?

86

Mobile Robotics

Let us now determine the singularities of our linearizing feedback loop. By calculating the expression of A(x), we can show that: 0 [2.11] / det (A(x)) = fr sin δr − v cos2 δr = v 2 sin2 δr − 1 .

We have a singularity when this quantity is equal to zero, i.e. if: v = 0 or δr =

π π +k . 4 2

[2.13]

The singularity corresponding to v = 0 is relatively simple to understand: when the boat is not advancing, we can no longer control it. The condition on the rudder angle δr is more delicate to interpret. Indeed, the condition δr = ± π4 translates to a maximal rotation. Any action on the rudder when δr = ± π4 translates to a slower rotation. This is what this singularity means. We are dealing with two decoupled monovariate systems here. Let us denote by ˆ in order to w = (w1 , w2 ) the setpoint for y. We will sometimes write w = (δˆs , θ) recall that w1 and w2 are the setpoints corresponding to the sail opening angle and the heading. Let us choose the proportional and derivative controller given by: !

v1 = (w1 − y1 ) + w˙ 1 ... v2 = (w2 − y2 ) + 3 (w˙ 2 − y˙ 2 ) + 3 (w ¨2 − y¨2 ) + w 2

which allows all poles of the feedback system to be equal to −1 (see [2.2]). By assuming that the setpoint w is constant, the state equations of the state feedback controller for our nonlinear system are given by: "" # # w 1 − δs −1 u = A (x) − b (x) . [2.14] w2 − θ − 3θ˙ − 3θ¨ But θ˙ and θ¨ are analytic functions of the state x. Indeed, we have: θ˙ = ω θ¨ = (1 − cos δs ) fs − cos δr fr − ω Equation [2.14] can, therefore, be written in the form: . u = r (x, w) = r x, δˆs , θˆ This controller is static since it does not have a state variable.

[2.15]

Feedback Linearization

87

2.6.4. Polar curve control In some situations, the boater does not want complete autonomy of his boat, only steering assistance. He does not wish to decide the angle of the sails, but simply its speed and heading. In summary, he would like to choose a point on the polar curve and it is up to the controller to perform low-level control. In cruising regime, we have:  0    0 f¯    ¯s fr

= = = =

f/¯s sin δ¯s − 0f¯r sin δ¯r − v¯ 1 −/ cos δ¯s 0 · f¯s − cos δ¯r · f¯r cos θ¯ + δ¯s − v¯ sin δ¯s v¯ sin δ¯r

/ 0 / 0 ¯ v¯ is in the polar curve, we can calculate f¯r , δ¯r , f¯s , δ¯s – there is at least If θ, / 0 ¯ δ¯s in one solution, by definition of the polar curve. Thus, it is sufficient to inject θ, controller [2.15] in order to preform our control. Figure 2.10 illustrates the docking of a sailboat in a harbor using this approach [HER 10].

Figure 2.10. By using a linearizing controller, the robot docks in its place in the harbor; the polar curve is represented on the top right corner. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

2.7. Sliding mode Sliding mode is a set of control methods that belong to the family of feedback linearization techniques. Assume that the system to be controlled has the form !

x˙ = f (x) + g (x) · u y = h (x)

88

Mobile Robotics

where dim u = dim y. Recall that, after computing some derivatives (see equation [2.7]), we get 

 (k ) y1 1  .   .  = A (x) · u + b(x).  .  (k ) ym m

. (k ) (k ) This means that, if A (x) is invertible, we can choose y(k) = y1 1 , . . . , ym m independently. For this, it suffices to take   (k ) y1 1 .   .  (k1 ) (km )    . u = A−1 (x)   ..  − b(x) = φ x, y1 , . . . , ym (km ) ym 

. (k ) (k ) Now, it remains to choose which vector y1 1 , . . . , ym m we need in order to achieve our control goal. Two different types of approach can be considered for this purpose. The first is the proportional and derivative control approach seen previously in this chapter. The second is the sliding surface method which will be explained in this section. For simplicity and without loss of generality, let us assume that we have a single input and a single output, i.e. dim u = dim y = 1. We thus have u=

. y (k) − b(x) = φ x, y (k) . a (x)

[2.16]

Proportional and derivative control method. We choose y (k) as follows: . (k) (k−1) − y (k−1) + yd . y (k) = α0 (yd − y) + α1 (y˙ d − y) ˙ + · · · + αk−1 yd If the αi are chosen well, then y (t) will quickly be equal to the desired output y (t). Equivalently, the error e = yd − y converges toward 0 with the dynamic e(k) + αk−1 e(k−1) + · · · + α1 e˙ + α0 e = 0.

Feedback Linearization

89

The corresponding feedback is . . (k−1) (k) u = φ x, α0 (yd − y) + · · · + αk−1 yd − y (k−1) + yd .

Sliding mode method.Instead of choosing a dynamic for y of order k, we choose a dynamic of order k − 1: y (k−1) = α0 (yd − y) + α1 (y˙ d − y) ˙ + ... . (k−2) (k−1) +αk−2 yd − y (k−2) + yd ,

or equivalently:

(k−1)

s (x, t) = yd 2

(k−2)

− y (k−1) + αk−2 (yd 34 5 2

e(k−1)

+ α0 (yd − y) = 0. 2 34 5

− y (k−2) ) + · · · + α1 (y˙ d − y) ˙ 2 34 5 34 5

e(k−2)

e˙

e

This equation corresponds to the dynamics of the error, but can also be interpreted as a surface of dimension k − 1 of the k-dimensional space (y, y, ˙ y¨, . . . , y (k−1) ). This surface is moving when yd is time-dependent. We were able to write this surface under the form s (x, t) = 0 because the elements (y, y, ˙ y¨, . . . , y (k−1) ) are all functions of x, (k) which is not the case for y , which depends on both x and u. Now, it remains to decide what we have to put for y (k) in equation [2.16]. The principle of sliding mode is to bring the system to this surface s (x, t) to stay on it. If we achieve this goal, then the error yd − y will have the required dynamics. To converge to the surface, we may take y (k) = K · sign (s (x, t)) , where K > 0 is large. If s > 0, it means that y (k−1) is too small and we have to increase it. This is why K should be positive. From equation [2.16] the sliding mode control will be u = φ (x, K · sign (s (x, t))) . One important feature of sliding mode is that we do not need to insert an integral effect to compensate for constant perturbations and the controller is robust with respect to many types of perturbation. Indeed, any perturbation that would escape the system

90

Mobile Robotics

from the sliding surface yields an immediate and strong reaction from the controller which brings it back to the surface. On the other hand, a sliding mode control generates a behavior with a high-frequency and non-deterministic switching control signal that causes the system to chatter in the close vicinity of the sliding surface. 2.8. Kinematic model and dynamic model 2.8.1. Principle The dynamic models for the robots are of the form: x˙ = f (x, u) where u is the vector of the external forces (that are under our control). The function f involves dynamic coefficients (such as masses, inertial moments, coefficients of friction, etc.) as well as geometric coefficients (such as lengths). The dynamic coefficients are generally not well known and can change in time with wear or usage. If we now take as new input the vector a of the desired accelerations at the application points of the forces (in the direction of the forces), we obtain a new model, referred to as kinematic, of the form: x˙ = ϕ (x, a) but in this new model most of the dynamic coefficients have disappeared. It is possible to switch from a dynamic model to a kinematic model using a so-called high-gain controller, of the form: u = K (a − a (x, u)) where K is a very large real number. The function a (x, u) is a function that allows to obtain the acceleration associated with the forces, in function of the forces u and of the state x. In practice, we are not looking to express a (x, u) within the controller, but rather to measure a (x, u) using accelerometers. The controller that will actually be implemented is: @) u = K (a − a

@ corresponds to the vector of the measured accelerations. Thus, we have: where a x˙ = f (x, K (a − a (x, u))) ⇔ x˙ = ϕ (x, a) .

Feedback Linearization

91

The simple high-gain feedback has allowed us to get rid of numerous dynamic parameters and switch from an uncertain system to a reliable one, with well-known geometric coefficients. This high-gain feedback is known in electronics as an operational amplifier, where it is used with the same idea of robustness. Switching from a dynamic model to a kinematic one has the following advantages: – the linearizing controller developed in this chapter requires using a reliable model such as a kinematic model. If the coefficients (which are not measured) are not well-known (such as in the case of dynamic systems), the linearizing controller will not work in practice; – the kinematic model is easier to put into equations. It is not necessary to have a dynamic model to obtain the latter; – the servo-motors (see section 2.8.3) are cheap and easy to find. They incorporate this high-gain controller. We may see them as mechanical operational amplifiers. We will illustrate the concept in the following paragraph, through the example of the inverted rod pendulum. 2.8.2. Example of the inverted rod pendulum Let us consider the inverted rod pendulum, composed of a pendulum in an unstable equilibrium on top of a moving cart, as represented on Figure 2.11.

Figure 2.11. Inverted rod pendulum to be modeled and controlled

92

Mobile Robotics

2.8.2.1. Dynamic model The quantity u is the force exerted on the cart of mass M , x indicates the position of the cart, θ is the angle between the pendulum and the vertical direction. The state equations are written in the form:       x˙ 0 x ˙   θ    0    d θ   u. =  −m sin θ()θ˙2 −g cos θ)  +  [2.17] 1 dt  x    ˙   M +m sin2 θ M +m sin2 θ cos θ sin θ((M +m)g−m)θ˙ 2 cos θ) θ˙ )(M +m sin2 θ) 2 )(M +m sin θ)

2.8.2.2. Kinematic model Let us recall the state equations of the inverted rod pendulum, but instead of taking the force as input, we take the acceleration a = x ¨. We obtain, following [2.17]: . 1 ˙2 − g cos θ) + u a= −m sin θ(* θ [2.18] M + m sin2 θ Therefore: 2 [2.17] sin θ((M + m)g − m*θ˙ cos θ) cos θ θ˙ = + u 2 *(M + m sin θ) *(M + m sin2 θ) 2 [2.18] sin θ((M + m)g − m*θ˙ cos θ) = 2 *(M + m sin- θ) . / 0 . cos θ + m sin θ *θ˙2 − g cos θ ) + M + m sin2 θ a 2 *(M + m sin θ) / 0 (M + m)g sin θ − gm sin θ cos2 θ + M + m sin2 θ cos θ · a = *(M + m sin2 θ) g sin θ cos θ = + a. * *

Let us note that this relation could have been obtained directly by noticing that: *θ¨ = a · cos θ + g · sin θ. N OTE.– In order to obtain this relation in a more rigorous manner, we need to write the temporal derivative of the speed composition formula, i.e. → −−→ − v˙ A = v˙ B + AB ∧ ω ˙ and write this formula in the coordinate system related to the pendulum. We obtain:         −g sin θ 0 0 a cos θ  −a sin θ  =  +*∧0 n 0 0 0 ω˙

Feedback Linearization

93

where n corresponds to the normal acceleration of the mass m. We thus obtain the desired relation, as well as the normal acceleration n = −a sin θ which will not be used. Finally, the kinematic model is written as:       x˙ x 0    θ˙   0  d θ      = dt  x  0  +  1 a ˙ cos θ g sin θ θ˙ )

[2.19]

)

This model, referred to as the kinematic model, only involves positions, speeds and accelerations. It is a lot more simple than the dynamic model and involves less coefficients. However, it does not correspond to reality since the actual input is a force and not an acceleration. In practice, we may switch from the dynamic model [2.17] with input u to the kinematic model [2.19] with input a by calculating u using a highgain proportional controller of the form: u = K (a − x ¨)

[2.20]

with K very large and where a is a new input.

Figure 2.12. The inverted rod pendulum, looped by a high gain K, behaves like a kinematic model

The acceleration x ¨ can be measured using an accelerometer. If K is sufficiently large, we will of course have the controller u that will give us the desired acceleration a, i.e. we will have x ¨ = a. Thus, System [2.17] can be described by the state equations in [2.19] which do not involve any of the inertial parameters of the system. A controller designed over the kinematic model will, therefore, be more robust than one designed over the dynamic system since the controller will function for any values of m, M , the inertial momentums, friction, etc. Let us recall that this high-gain controller is very

94

Mobile Robotics

close to the principle of the operational amplifier. In addition to being more robust, such an approach allow us to have a simpler model that is easier to obtain. For the implementation of the controller in [2.20], we, of course, do not need to use the state equations [2.17] in order to express x ¨, but measure x ¨ instead. It is this measurement that allows us to have a controller that is independent of the dynamic parameters. Let us recall our inverted rod pendulum and try to make the pendulum oscillate from left to right with a desired angle of the form θd = sin t. Let us apply a linearizing controller for this. We have: g sin θ cos θ θ¨ = + a. * * We will, therefore, take: * a= cos θ

" # g sin θ v− *

where v is the new input. We will then choose: . v = (θd − θ) + 2 θ˙d − θ˙ + θ¨d = sin t − θ + 2 cos t − 2θ˙ − sin t.

And finally:

u = K (a − x ¨) " " # # * g sin θ =K sin t − θ + 2 cos t − 2θ˙ − sin t − −x ¨ . cos θ * Note that the inertial parameters are not taken into account in this controller. This controller ensures that the system will respect its setpoint angle. On the other hand, the position of the cart can diverge, since u does not depend on x. The dynamics of x are hidden and, moreover, unstable here. These hidden dynamics are conventionally referred to as zero dynamics. We will see in exercise 2.5 how the hidden dynamics can be avoided by choosing an output which maximizes the relative degree. 2.8.3. Servo-motors A mechanical system is controlled by forces or torques and obeys a dynamic system that depends on numerous little-known coefficients. This same mechanical system represented by a kinematic model is controlled by positions, speeds or accelerations. The kinematic model depends on well-known geometric coefficients and is much simpler to put into equations. In practice, one switches from a dynamic

Feedback Linearization

95

model to its kinematic equivalent by adding servo-motors. In summary, a servo-motor is a DC (Direct Current) motor with an electrical control circuit and a sensor (of position, speed or acceleration). The control circuit calculates the voltage u to give the motor in order for the quantity measured by the sensor to correspond to the setpoint w. There are three types of servo-motors: – the position servo. The sensor measures the position (or the angle) x of the motor and the control law is expressed by u = K (x − w) . If K is large, we may conclude that x . w;

– the speed servo. The sensor measures the speed (or the angular speed) x˙ of the motor and the control law is expressed by u = K (x˙ − w) . If K is large, we have x˙ . w;

– the acceleration servo. The sensor measures the acceleration (tangential or angular) x ¨ of the motor and the control law is expressed by u = K (¨ x − w) . If K is large, we have x ¨ . w. It is this type of servo-motor that we have chosen for the inverted rod pendulum. Thus, when we wish to control a mechanical system, the use of servo-motors allows us (i) to have a model that is easier to obtain, (ii) to have a model with fewer coefficients that is closer to reality and (iii) to have a more robust controller with respect to any modification of the dynamic coefficients of the system. 2.9. Exercises E XERCISE 2.1.– Crank Let us consider the manipulator robot, or crank of Figure 2.13 (on the left). This robot is composed of two arms of length *1 and *2 . Its two degrees of freedom denoted by x1 and x2 are represented on the figure. The inputs u1 , u2 of the system are the angular speeds of the arms (i.e. u1 = x˙ 1 , u2 = x˙ 2 ). We will take as output the vector y = (y1 , y2 ) corresponding to the end of the second arm. 1) Give the state equations of the robot. We will take the state vector x = (x1 , x2 ) . 2) We would like y to follow a setpoint w describing a target circle (on the right of Figure 2.13). This setpoint satisfies: w =c+r·

"

cos t sin t

#

.

96

Mobile Robotics

Figure 2.13. Manipulator robot whose end effector must follow a circle

Give the expression of a control law that allows to perform this task. We will use a feedback linearization method and we will place the poles at −1. 3) Study the singularities of the control.

4) Let us consider the case *1 = *2 , c = (3, 4) and r = 1. For which values of *1 are we certain to be able to move freely on the target circle, without encountering singularities? 5) Write a program illustrating this control law. E XERCISE 2.2.– The three pools Let us consider a flow system with three pools as represented on Figure 2.14.

Figure 2.14. System composed of three pools containing water and connected by two channels

Feedback Linearization

97

This system is described by the following state equations:  h˙ 1      h˙ 2 h˙ 3    y   1 y2

= = = = =

−α (h1 ) − α (h1 − h2 ) + u1 α (h1 − h2 ) − α (h2 − h3 ) −α (h3 ) + α (h2 − h3 ) + u2 h1 h3

: where α(h) = a·sign(h) 2g|h|. We have chosen our outputs here to be the water levels in the first and third pools. 1) Propose a feedback that would make the system linear and decoupled. 2) Propose a proportional-integral controller for the linearized system. 3) Give the state equations of the obtained controller. 4) Write a program that simulates the system and its control law. E XERCISE 2.3.– Train robot Let us consider a robot A (on the left of Figure 2.15) described by the following state equations (tank model):  x˙ a = va cos θa    y˙ = v sin θ a a a θ˙a = ua1    v˙ a = ua2

where va is the speed of the robot, θa its orientation and (xa , ya ) the coordinates of its center. We assume to be able to measure the state variables of our robot with very high precision. 1) Calculate x ¨a , y¨a in function of xa , ya , va , θa , ua1 , ua2 . 2) Propose a controller that allows to follow the trajectory: !

x ˆa (t) = Lx sin(ωt) yˆa (t) = Ly cos(ωt)

with ω = 0.1, Lx = 15 and Ly = 7. A feedback linearization method must be used for this. Illustrate the behavior of your controller with a program. 3) A second robot B of the same type as A wishes to follow robot A (see Figure 2.16). We define a virtual attachment point with coordinates (ˆ xb , yˆb ) in order for vehicle B (on the left of the figure) to be able to attach itself to our robot A. We can send it the information associated with the attachment point wirelessly.

98

Mobile Robotics

Figure 2.15. Our robot (with eyes) following a vehicle (here a car) whose state equations are unknown. This car has an imaginary attachment point (small white circle) that we must attach to

Figure 2.16. Robot B (dotted) has to follow robot A

This point will be positioned at the rear of robot A, at a distance * of our reference point (xa , ya ). Give the expression of these quantities in function of the state of our vehicle A. 4) Simulate this second vehicle B, together with its controller following robot A. 5) Add a third robot C that follows B with the same principle. Simulate the entire system. 6) Given that in this exercise the reference path is precisely known, propose a controller that will allow robots B and C to precisely follow robot A.

Feedback Linearization

99

E XERCISE 2.4.– Controlling a 3D underwater robot Let us consider the underwater robot already discussed in exercise 1.10. This robot is described by the following state equations:    p˙ x = v cos θ cos ψ   p˙ y = v cos θ sin ψ      p˙ z = −v sin θ v˙ = u1   ϕ ˙ = −0.1 sin ϕ · cos θ + tan θ · v · (sin ϕ · u2 + cos ϕ · u3 )    ˙  ϕ · v · u2 − sin ϕ · v · u3   θ = cos  cos ϕ ϕ ψ˙ = sin cos θ · v · u2 + cos θ · v · u3

where (px , py , pz ) is the position of its center and (ϕ, θ, ψ) are the three Euler angles. We took here u4 = −0.1 sin ϕ · cos θ in order to simulate a static ballast which tends to make the roll equal to zero via a pendulum effect. Its inputs are the tangential acceleration u1 , the pitch u2 and the yaw u3 . Suggest a controller capable of controlling the robot around the cycloid of equation:     R · sin(f1 t) + R · sin(f2 t) xd  yd  =  R · cos(f1 t) + R · cos(f2 t)  zd R · sin(f3 t) with f1 = 0.01, f2 = 6f1 , f3 = 3f1 and R = 20. For the control, we will choose a time constant of five seconds. Simulate the behavior of the controller. E XERCISE 2.5.– Reaction wheel pendulum

The Reaction Wheel Pendulum [SPO 01], as shown in Figure 2.17 (left), is a physical pendulum with a disk attached to the end. The disk can rotate and is actuated by a motor. The coupling torque generated by the angular acceleration of the disk can be used to actively control the pendulum, for instance to maintain it standing. This problem is very similar to tightrope walker stabilization (Figure 2.17 (right)), where the rod plays the role of the disk and the walker corresponds to the pendulum. We assume that the system can be described by the following equations:   x˙ 1 = x2 x˙ 2 = a · sin(x1 ) − b · u  x˙ 3 = −a · sin(x1 ) + c · u

where x1 is the pendulum angle, x2 is its angular velocity, x3 is the disk angular velocity and u is the motor torque input. The parameters are taken as a = 10, b = 1, c = 2 and depend on the mass, the inertia and the dimensions of the system.

100

Mobile Robotics T

1) Simulate the system with an initial condition of x = (1 0 0) . 2) Taking as an output y = x1 , stabilize the pendulum at the top (i.e. x1 = 0). Explain why the wheel never stops. 3) Taking as an output of the form y = α 1 x 1 + α 2 x2 + α 3 x3 where α1 , α2 , α3 should be chosen accordingly, stabilize the pendulum at the top (i.e. x1 = 0) with a motionless wheel (or with a wheel which rotates at a speed ω).

Figure 2.17. Reaction wheel pendulum

E XERCISE 2.6.– Pursuit Let us consider two robots described by the following state equations:   x˙ 1 = u1 cos θ1 y˙ 1 = u1 sin θ1 ˙ θ 1 = u2

  x˙ 2 = v1 cos θ2 y˙ 2 = v1 sin θ2 and ˙ θ2 = v2

In this exercise, robot 1 tries to follow robot 2 (see Figure 2.18). 1) Let x = (x, y, θ) be the position vector of robot 2 in the coordinate system of robot 1. Show that x satisfies a state equation of the form x˙ = f (x, v, u) 2) We assume that the control variables v1 and v2 of robot 2 are known (a polynomial in t, for example). Suggest a controller that generates us u in order to have x = w1 and y = w2 , where w = (w1 , w2 ) corresponds to a setpoint in relative position. The poles for the error are fixed at −1.

Feedback Linearization

101

Figure 2.18. Robot 1 is in pursuit of robot 2

3) Study the singularities of this controller. 4) Illustrate this control law in the situation where robot 1 would like to point towards robot 2 while keeping a distance of 10 m. E XERCISE 2.7.– Controlling the S AUCISSE robot Consider the underwater robot represented on Figure 2.19. This is the S AUCISSE robot, built by students of the E NSTA Bretagne for the SAUC’E competition (Student Autonomous Underwater Challenge Europe). It includes three propellers. Propellers 1 and 2 on the left and the right are able to act on the speed of the robot and its angular speed. Propeller 3 acts on the depth of the robot. This robot is stable in roll and pitch, and we will assume that its angles of bank ϕ and elevation θ are always zero. The state equations of the robot are the following:  x˙     y˙     z˙    ˙ ψ v˙ x     v˙ y     v˙    z ω˙

= = = = = = = =

vx vy vz ω u1 cos ψ u1 sin ψ u3 u2

102

Mobile Robotics

Figure 2.19. The S AUCISSE robot in a pool

Let us note that no nonholonomic constraint has been assumed in this model. The speed vector of the robot (vx , vy ) is not necessarily in its axis, in contrast to the case of the cart model. The robot can therefore operate in crab steering mode. On the other hand, the propulsion is necessarily in the direction of the robot axis. If we are limited to the horizontal plane, this model is known as a hovercraft. 1) Give the differential dependency graph associated with this system. 2) Let us choose as output the vector y = (x, y, z). Give the differential delay matrix and deduce the relative degrees from it. What can we conclude? 3) In order to balance the differential delays by delaying u1 , we add two integrators in front of u1 . Our new system will admit as new inputs a = (a1 , a2 , a3 ) with:  ¨1 = a1 u u2 = a2 [2.21]  u3 = a3

What are the new state equations of the delayed system? Give the differential dependency graph as well as the associated differential delay matrix. 4) Perform a feedback linearization of the delayed system. 5) Deduce from above the controller corresponding to our robot. We will place all the poles at −1.

Feedback Linearization

103

E XERCISE 2.8.– Sliding mode control of a cart Consider the cart described by  x˙ 1 = x4 cos x3    x˙ 2 = x4 sin x3 x˙ 3 = u1    x˙ 4 = u2

where (x1 , x2 ) corresponds to the position of the cart, x3 to its heading and x4 to its speed. 1) Provide a controller based on a feedback linearization to make the cart follow the Lissajou trajectory: yd (t) = 10 ·

"

cos t sin 3t

#

.

Illustrate the behavior of your controller. 2) Now implement a sliding mode controller which makes the cart follow the desired Lissajou trajectory. Compare it with Question 1. E XERCISE 2.9.– Group of robots Consider a group of m = 20 carts, the motion of which is described by the state equation  x˙ 1 = x4 cos x3    x˙ 2 = x4 sin x3 x˙ 3 = u1    x˙ 4 = u2

where (x1 , x2 ) corresponds to the position of the cart, x3 to its heading and x4 to its speed. 1) Provide a controller for each of these robots so that the ith robot follows the trajectory "

cos(at + 2iπ m ) sin(at + 2iπ m )

#

.

where a = 0.1. As a consequence, after the initialization step, all robots are uniformly distributed on the unit circle, turning around the origin. 2) By using a linear transformation of the unit circle, change the controllers for the robots so that all robots stay on a moving ellipse with the first axis of length

104

Mobile Robotics

20 + 15 · sin(at) and the second axis of length 20. Moreover, we make the ellipse rotate by choosing an angle for the first axis of θ = at. Illustrate the behavior of the controlled group. E XERCISE 2.10.– Convoy Let us consider one robot RA described by the following state equations:  x˙ a = va cos θa    y˙ = v sin θ a a a θ˙a = ua1    v˙ a = ua2

where va is the speed of the robot RA , θa is its orientation and (xa , ya ) are the coordinates of its center. 1) As for exercise 2.3, propose a controller for RA that allows it to follow the trajectory: !

x ˆa (t) = Lx sin(ωt) yˆa (t) = Ly cos(ωt)

with ω = 0.1, Lx = 20 and Ly = 5. Illustrate the behavior of the control with a sampling time dt = 0.03 sec. 2) We want m = 6 other robots with the same state equations to follow this robot, taking exactly the same path. The distance between two robots should be d = 5 m. To achieve this goal, we propose to save the value of the state of RA every ds = 0.1 m and to communicate this information to the m followers. To synchronize the time with the traveled distance, we propose to add a new state variable s to RA which corresponds to the curvilinear value that could have been measured by a virtual odometer. Each time the distance ds is measured by the virtual odometer, s is initialized to zero and the value for the state of RA is broadcast. Simulate the behavior of the group. E XERCISE 2.11.– Profiling float We consider an underwater profiling float corresponding to a cube of width * which is immersed in the water (see Figure 2.20) of density ρ0 . The float at a depth d moves up and down at a speed equal to v. The total force applied to the float is 1 f = mg − ρ0 g*2 · max (0, * + min(d, 0)) − ρ0 v· | v | *2 cx 2345 2 34 5 22 34 5 fg ↓ f ↑ a

where

fd +

Feedback Linearization

105

– fg is the gravity force, m is the mass of the float and g is the gravity. – fd is the drag force. For a cube, we can consider that cx . 1.05.

– fa is the Archimedes’ force. The buoyant force is exerted upward on the body, whether fully or partially submerged. It is equal to the weight of the water that the body displaces. The volume of displaced water is *2 · max (0, * + min(d, 0)).

Figure 2.20. Underwater float controlled by a ballast

For the float, a ballast allows us to control the density of the body, which is (1 + βb) ρ0 , where β = 0.1. The derivative u of the buoyancy variable b can be fixed using a DC-motor controlling a piston. We assume that both u and b belong to [−1, 1]. When b = 0, the density of the float is exactly that of the water ρ0 . When b > 0, the float sinks and when b < 0, it surfaces. 1) Write the state equations of the system x˙ = f (x, u). The state vector will be taken as x = (d, v, b). 2) Assume that the float is immersed, and find a sliding mode controller so that the float has a depth d (t) equal to the desired depth d0 (t). 3) Illustrate the behavior of your controller using a simulation. First, control the float at a depth d0 (t) = 5 m. Then control the float at d0 (t) = 3 + sin(t/2). 4) Compare your controller to a feedback linearization-based controller. E XERCISE 2.12.– Field following We consider a mobile robot with an input vector u = (u1 , . . . , um ) and a state vector x = (x1 , . . . , xn ), n ≥ m + 1. In this exercise, we show that if we select m + 1 state variables, say p = (x1 , . . . , xm+1 ), we can follow a chosen vector field in the p = (x1 , . . . , xm+1 ) space, using a feedback linearization method. This means that we can control m + 1 state variables and not m of them, as given by the theory. This is due to the fact that we perform a path following, instead of a trajectory following where the time is involved. More precisely, we ask for the vector p˙ = (x˙ 1 , . . . , x˙ m+1 ) to be collinear (instead of equal) to the required field. This is illustrated in this exercise in the case where u is scalar, i.e. m = 1.

106

Mobile Robotics

Consider a robot moving on a plane and described by the following state equations:   x˙ 1 = cos x3 x˙ 2 = sin x3  x˙ 3 = u

where x3 is the heading of the robot and p = (x1 , x2 ) are the coordinates of its center. The state vector is given by x = (x1 , x2 , x3 ). 1) Take as an output y = x3 + atanx2 . Propose a feedback linearization-based controller such that the output converges to 0. What are the singularities of the control? 2) Assume that y(t) converges to 0, to which line the robot will converge? Draw the vector field followed by the robot in the (x1 , x2 )-space. 3) We would like the robot to follow a path corresponding to the limit cycle of the Van der Pol equation: !

x˙ 1 = x2/ 0 , x˙ 2 = − 0.01 · x21 − 1 x2 − x1

Find the corresponding feedback-linearization-based controller. Propose a simulation to illustrate the behavior of the controller. E XERCISE 2.13.– Sliding pendulum Consider the pendulum represented by Figure 2.21 and described by "

x˙ 1 x˙ 2

#

=

"

x2 − sin x1 + u

#

,

where u is the input, x1 its position and x2 its angular velocity. We would like the position x1 (t) of the pendulum to converge to the setpoint w (t) = 0, ∀t.

1) Taking y = x1 as an output, propose a feedback linearization method to control the pendulum so that the error e = w − y converges toward 0 as exp(−t). 2) Give an answer to the previous question with a sliding mode approach.

3) We want to compute a value for K so that the surface s (x, t) = 0 is stable. More precisely, if we are near to the surface, then in a near future, we want to be even closer.

Feedback Linearization

107

To show this stability, a Lyapunov approach defines a distance function, e.g. V (x) = 1 2 2 s (x, t). This function is positive and vanishes on the surface. Demonstrating the stability amounts to proving that V (x) decreases when we are near to the surface. Compute V˙ (x), which also depends on K, and conclude.

Figure 2.21. Simple pendulum with state vector x = (x1 , x2 )

2.10. Corrections C ORRECTION FOR E XERCISE 2.1.– (Crank) 1) The state equations of the crank are:  x˙ 1 = u1    x˙ 2 = u2 y1 = *1 cos x1 + *2 cos (x1 + x2 )    y2 = *1 sin x1 + *2 sin (x1 + x2 )

2) By differentiating the output, we obtain: y˙ 1 = −*1 x˙ 1 sin x1 − *2 (x˙ 1 + x˙ 2 ) sin (x1 + x2 ) = −*1 u1 sin x1 − *2 (u1 + u2 ) sin (x1 + x2 ) y˙ 2 = *1 x˙ 1 cos x1 + *2 (x˙ 1 + x˙ 2 ) cos (x1 + x2 ) = *1 u1 cos x1 + *2 (u1 + u2 ) cos (x1 + x2 ) . Thus: "

# −*1 sin x1 − *2 sin (x1 + x2 ) −*2 sin (x1 + x2 ) u. y˙ = *1 cos x1 + *2 cos (x1 + x2 ) *2 cos (x1 + x2 ) 2 34 5 A(x)

108

Mobile Robotics

We take u = A−1 (x) · v to have two decoupled integrators. We then choose the proportional controller: v = (w − y) ˙ # " "+ w # " # cos t *1 cos x1 + *2 cos (x1 + x2 ) − sin t = c+r· − +r· sin t *1 sin x1 + *2 sin (x1 + x2 ) cos t which places all of our poles at −1.

3) By multilinearity of the determinant, we have: " # sin x1 sin (x1 + x2 ) det A (x) = −*1 *2 det cos x1 cos (x1 + x2 ) 2 34 5 =sin x2 " # − sin (x1 + x2 ) − sin (x1 + x2 ) 2 +*2 det cos (x1 + x2 ) cos (x1 + x2 ) 2 34 5 =0

This determinant is equal to zero if *1 *2 sin x2 = 0. This happens if x2 = kπ, kZ or if one of the two arms is of length zero. 4) If *1 = *2 &= 0, we have a singularity if sin x2 = 0. Thus, either both arms are folded up (and therefore y is not on the circle) or both arms are stretched out (see Figure 2.22). In the latter case (which is of interest to us) the point y is on the circle of radius 2*1 that intersects the target circle if: *1 + *2 ∈

: 42 + 32 ± 1 = 5 ± 1 = [4, 6]

√ where 42 + 32 corresponds to the distance of the center of the circle to the origin. We will have a singularity on the circle if *1 = *2 ∈ [2, 3]. If we wish to move freely on the circle, we need to choose *1 = *2 > 3. C ORRECTION FOR E XERCISE 2.2.– (The three pools) 1) The derivatives of the outputs y1 and y2 are expressed by: y˙ 1 = h˙ 1 = −α (h1 ) − α (h1 − h2 ) + u1 y˙ 2 = h˙ 3 = −α (h3 ) + α (h2 − h3 ) + u2 or, in vector form: "

y˙ 1 y˙ 2

#

=

# # " −α (h1 ) − α (h1 − h2 ) 1 0 u+ −α (h3 ) + α (h2 − h3 ) 0 1 2 34 5 2 34 5 "

A(x)

b(x)

Feedback Linearization

109

Figure 2.22. Illustration of a singularity, when both arms are stretched out. The end effector might not be able to follow the circle

The feedback: u = A−1 (x) (v − b (x)) = v −

"

−α (h1 ) − α (h1 − h2 ) −α (h3 ) + α (h2 − h3 )

#

where v is our new input, makes our system linear. More precisely, the system looped in such a way has the form: ! y˙ 1 = v1 y˙ 2 = v2 2) Let us now try to control this linear system by a system composed of two PI (proportional-integral) controllers of the form: A Bt v1 (t) = α0 (w1 (t) − y1 (t)) + α−1 0 (w1 (τ ) − y1 (τ )) dτ + w˙ 1 Bt v2 (t) = β0 (w2 (t) − y2 (t)) + β−1 0 (w2 (τ ) − y2 (τ )) dτ + w˙ 2 where w1 and w2 are the new setpoints for y1 and y2 . If we want all our poles to be equal to −1, we need: ! 2 2 s + α0 s + α−1 = (s + 1) = s2 + 2s + 1 2 s2 + β0 s + β−1 = (s + 1) = s2 + 2s + 1 Thus α−1 = β−1 = 1, α0 = β0 = 2. 3) The state equations for the controller are therefore:  z˙1 = w1 − y1    z˙2 = w2 − y2 v1 = z1 + 2 (w1 − y1 ) + w˙ 1    v2 = z2 + 2 (w2 − y2 ) + w˙ 2 .

110

Mobile Robotics

The state equations of a state feedback controller for our nonlinear system are therefore:    z˙1 = w1 − h1  z˙2 = w2 − h3 u ˙ 1 + α (h1 ) + α (h1 − h2 )  1 = z1 + 2 (w1 − h1 ) + w   u2 = z2 + 2 (w2 − h3 ) + w˙ 2 + α (h3 ) − α (h2 − h3 ) . C ORRECTION FOR E XERCISE 2.3.– (Train robot)

1) We have: ! x ¨a = v˙ a cos θa − va θ˙a sin θa = ua2 cos θa − va ua1 sin θa y¨a = v˙ a sin θa + va θ˙a cos θa = ua2 sin θa + va ua1 cos θa . Therefore: #" " # " # x ¨a −va sin θa cos θa ua1 = va cos θa sin θa y¨a ua2 2 34 5 A(va ,θa )

2) By using a feedback linearization method, we obtain: .   # " xa − v cos θ + d2 x (ˆ xa − xa ) + 2 dˆ ˆa a a 2 ua1  - dt . dt2 = A−1 (va , θa ) ·  dˆ ya ua2 (ˆ ya − ya ) + 2 − va sin θa + d 2 yˆa dt dt   Lx sin ωt − xa + 2ωLx cos ωt  −2va cos θa − ω 2 Lx sin ωt  −1  = A (va , θa ) ·  Ly cos ωt − ya − 2ωLy sin ωt +2va sin θa − ω 2 Ly cos ωt

3) Let us apply the controller obtained in the previous question. It is given by:   . 2 " # dˆ x d x ˆ b b xb − x b ) + 2 − vb cos θb + 2  ub1  (ˆ dt - dt . = A−1 (vb , θb ) ·   2 ub2 dˆ yb d yˆ (ˆ yb − yb ) + 2 − vb sin θb + 2b dt dt We have:

x ˆb = xa − * cos θa yˆb = ya − * sin θa d x ˆb = x˙ a + *θ˙a sin θa = va cos θa + *u1a sin θa dt d yˆb = y˙ a − *θ˙a cos θa = va sin θa − *u1a cos θa . dt

Feedback Linearization 2

111

2

d d In order to have dt ˆb , dt ˆb , we would need to have u˙ 1a , which is not the case. 2x 2y Here, we will simply assume that these two quantities are equal to zero and hope that this approximation will not lead to the instability of the system. We are, therefore, no longer assured of an exponential convergence of the error towards 0. However, we will note that in practice, the behavior remains acceptable.

5) We simply need to recall the same controller as in the previous question. Figure 2.23 illustrates the behavior of our train. Cart A is turning in an ellipse, cart B follows cart A and cart C follows cart B.

Figure 2.23. Illustration of the execution of the program; cart A is in bold grey, cart B is in light grey and cart C is in white; the setpoints and attachment points are represented by small circles

6) We simply need to take completely independent controllers with a simple temporal delay, for example: "

x ˆa yˆa

#

# " # " # Lx sin ωt x ˆb Lx sin ω (t − 1) , = Ly cos ωt yˆb Ly cos ω (t − 1) " # " # x ˆc Lx sin ω (t − 2) and = yˆc Ly cos ω (t − 2)

=

"

112

Mobile Robotics

This control law can be considered centralized since it requires a supervisor capable of sending coherent setpoints to all the robots. C ORRECTION FOR E XERCISE 2.4.– (Controlling a 3D underwater robot) Let us choose as output the position vector of the robot p. We have: 

 v˙ cos θ cos ψ − v θ˙ sin θ cos ψ − v ψ˙ cos θ sin ψ p ¨ (t) =  v˙ cos θ sin ψ − v θ˙ sin θ sin ψ + v ψ˙ cos θ cos ψ  −v˙ sin θ − v θ˙ cos θ    v˙ cos θ cos ψ −v cos θ sin ψ −v sin θ cos ψ −v sin θ sin ψ   ψ˙  =  cos θ sin ψ v cos θ cos ψ − sin θ 0 −v cos θ θ˙   cos θ cos ψ −v cos θ sin ψ −v sin θ cos ψ −v sin θ sin ψ  =  cos θ sin ψ v cos θ cos ψ − sin θ 0 −v cos θ % &' ( 

A(x)

  1 0 0 u1 v sin ϕ cos ϕ 0 v cos θ   u2  cos θ u3 0 v cos ϕ −v sin ϕ

By looping the system using the controller u = A−1 (x)v, we obtain the decoupled linear system p ¨ = v. This system has three inputs and three outputs. If w = (w1 , w2 , w3 ) represents the path to follow for p, we may consider the following controller: v = 0.04 · (w − p) + 0.4 · (w ˙ − p) ˙ +w ¨

in order for all the poles to be equal to −0.2 (which corresponds to a characteristic 2 polynomial P (s) = (s + 0.2) ). The state feedback controller for our robot is therefore:      v cos θ cos ψ u = A−1 (x) · 0.04 · (w − p) + 0.4 · w ˙ −  v cos θ sin ψ  + w ¨ −v sin θ

with



 R sin(f1 t) + R sin(f2 t) w =  R cos(f1 t) + R cos(f2 t)  R sin(f3 t)   Rf1 cos(f1 t) + Rf2 cos(f2 t) w ˙ =  −Rf1 sin(f1 t) − Rf2 sin(f2 t)  Rf3 cos(f3 t)   2 −Rf1 sin(f1 t) − Rf22 sin(f2 t) w ¨ =  −Rf12 cos(f1 t) − Rf22 cos(f2 t)  −Rf32 sin(f3 t)

Feedback Linearization

113

C ORRECTION FOR E XERCISE 2.5.– (Reaction wheel pendulum) 2) We have   y = x1 y˙ = x2  y¨ = a sin(x1 ) − bu

Thus, the control u=

a sin(x1 ) − v , b

where v = (yd − y) + 2 (y˙ d − y) ˙ + y¨d , will stabilize the pendulum angle y = x1 along the desired time-dependent angle yd (t). For yd = y˙ d = y¨d = 0, we get that x1 and x2 converge to zero. But x3 may converge to any value. The reason for this is that the closed loop system is of dimension 2 (the state variables are y = x1 and y˙ = x2 ) and one state (here x3 ) is totally free. The free evolution of x3 corresponds to what is called the zero dynamics. 3) The idea is to choose α1 , α2 , α3 in order to delay the occurrence of u. Equivalently, we want a relative degree of 3, in order to avoid any zero dynamics. We have y˙ = α1 x2 + α2 (a sin(x1 ) − bu) + α3 (−a sin(x1 ) + cu) . To avoid the dependency with respect to u, we want α2 b = α3 c. We take α2 = c and α3 = b. Thus, y˙ = α1 x2 + a (c − b) sin(x1 ). 2 34 5 η

We have

y¨ = α1 x˙ 2 + η x˙ 1 cos(x1 ) = α1 (a sin(x1 ) − bu) + ηx2 cos(x1 ). To avoid the dependency with respect to u, we take α1 = 0. Thus, y¨ = ηx2 cos(x1 ).

114

Mobile Robotics

We have ... y = = = =

η x˙ 2 cos(x1 ) − ηx2 x˙ 1 sin(x1 ) η (a sin(x1 ) − bu) cos(x1 )/− ηx22 sin(x1 ) 0 −bη cos(x1 )u + η sin(x1 ) a cos(x1 ) − x22 a(x)u + b(x).

We take u=

−b(x) + v . a(x)

A singularity exists when a(x) = 0, i.e. if cos(x1 ) = 0. For v, we can take (see equation [2.2]): ... v = (yd − y) + 3 (y˙ d − y) ˙ + 3 (¨ yd − y¨) + y d . If ∀t, yd (t) = 0, then yd = y˙ d = y¨d = 0. The variables y = y˙ = y¨ all converge to zero. Since we have     y cx2 + bx3  y˙  =  a (c − b) sin x1  , y¨ a (c − b) x2 cos x1

the quantities sin x1 , x2 , x3 also converge to zero and the wheel will stop. But x1 may converge ... to 0 or to π, depending of the initialization. Note that for yd = bω, y˙ d = y¨d = y d = 0, the variable x3 (rotation speed of the wheel) will converge to ω. C ORRECTION FOR E XERCISE 2.6.– (Pursuit) 1) The relative distance between the two robots (i.e. the position of robot 2 expressed in the coordinate system of robot 1) is written as:      x cos θ1 sin θ1 0 x2 − x1  y  =  − sin θ1 cos θ1 0   y2 − y1  . θ 0 0 1 θ2 − θ1

Let us differentiate the first two components of this relation with respect to t. We obtain: " # " #" # x˙ cos θ1 sin θ1 x˙ 2 − x˙ 1 = y˙ − sin θ1 cos θ1 y˙ 2"− y˙ 1 " # # − sin θ cos θ x2 − x1 1 1 ˙ +θ1 . − cos θ1 − sin θ1 y2 − y1

Feedback Linearization

However:  ˙   " θ#1   x˙ 2      " y˙ 2 # x˙ 1   y˙ 1 #   "    x2 − x1   y2 − y1

= u2 " # cos θ2 = v1 " sin θ2 # cos θ1 = u1 " sin θ1 #" # cos θ1 − sin θ1 x = sin θ1 cos θ1 y

Therefore " # " #" # x˙ cos θ1 sin θ1 cos θ2 = v1 y˙ − sin θ1 cos θ1 sin θ2 #" # " #" # " cos θ1 0 1 x cos θ1 sin θ1 + u2 − u1 − sin θ1 cos θ1 sin θ1 −1 0 y " # " # " # 1 y cos (θ2 − θ1 ) + u2 = v1 − u1 0 −x sin (θ2 − θ1 ) " # −u1 + v1 cos (θ2 − θ1 ) + u2 y = . v1 sin (θ2 − θ1 ) − u2 x Moreover θ˙ = θ˙2 − θ˙1 = v2 − u2 . Thus     x˙ −u1 + v1 cos θ + u2 y .  y˙  =  v1 sin θ − u2 x ˙θ v2 − u 2

2) We have:

" # " # " #" # x˙ v1 cos θ −1 y u1 = + . y˙ v1 sin θ 0 −x u2 We propose the following controller: "

u1 u2

#

=

"

−1 y 0 −x

#−1 ""

w1 − x w2 − y

#

+

"

w˙ 1 w˙ 2

#

−

"

v1 cos θ v1 sin θ

##

.

115

116

Mobile Robotics

Thus, by combining the two previous equations, we obtain the equations on the error at x and y: "

w1 − x w2 − y

#

+

"

w˙ 1 − x˙ w˙ 2 − y˙

#

= 0.

The characteristic polynomial of these two errors is P (s) = s + 1, which tells us that the error converges towards 0 at e−t . 3) We have a singularity of our control law when: det

"

−1 y 0 −x

#

= 0,

i.e. when x = 0, and also when robot 2 is on the left or the right of robot 1. 4) The program is the following: 1 xa := (−10, −10, 0)T ;xb := (−5, −5, 0)T ; dt := 0.01 2 For t := 0 : dt : 10 3 (x ya , θa ):= xa ; (xb , yb , θb ) :=xb  a,  x cos θa sin θa 0  y  :=  − sin θa cos θa 0  · (xb − xa ) 4 θ " 0 # 0 " 1# " # 3 10 0 ˙ := 5 v := ; w := ;w sin(0.2 · t) 0 0 " #−1 " " # " ## −1 y x cos θ ˙ − v1 6 u := w− +w 0 −x y sin θ 7 xa := xa + f (xa , u) · dt; 8 xb := xb + f (xb , v) · dt; This program uses the following evolution function: 

 u1 cos x3 f (x, u) =  u1 sin x3  u2

C ORRECTION FOR E XERCISE 2.7.– (Controlling the S AUCISSE robot) 1) The differential dependency graph is given in Figure 2.24. 2) The differential delay matrix is: 

 2 4 ∞ R = 2 4 ∞ ∞ ∞ 2

Feedback Linearization

117

Figure 2.24. Differential dependency graph of the underwater robot

The relative degrees are k1 = 2, k2 = 2 and k3 = 2. However, since the second column is such that ∀i, ri2 > ki , the method of linearizing feedback will necessarily lead to a matrix A (x) that is singular for any state vector x (refer to section 2.3.3). 3) We obtain the following state equations for our new system:  x˙ = vx     y˙ = vy     z˙ = vz     ψ˙ = ω    v˙ x = c1 cos ψ v˙ y = c1 sin ψ     v˙ z = a3     ω˙ = a2     c˙1 = b1   ˙ b1 = a1

where b1 and c1 are the new state variables attached to our integrators. The differential dependency graph is given in Figure 2.25. The differential delay matrix is given by:   4 4 ∞ R = 4 4 ∞. ∞ ∞ 2 The relative degrees are k1 = 4, k2 = 4 and k3 = 2.

.... x 4) ....In order to linearize the delayed system by feedback, we need to calculate and y in function of x and u. We have:  x ¨ = c1 cos ψ   ...   x = b1 cos ψ − c1 ω sin ψ   ....    x = a1 cos ψ − 2b1 ω sin ψ − c1 a2 sin ψ − c1 ω 2 cos ψ y... ¨ = c1 sin ψ   y   .... = b1 sin ψ + c1 ω cos ψ   y = a1 sin ψ + 2b1 ω cos ψ + c1 a2 cos ψ − c1 ω 2 sin ψ    z¨ = a3

118

Mobile Robotics

Figure 2.25. Differential dependency graph after adding the integrators

Therefore:  ....       2 x cos ψ −c sin ψ 0 a −2b ω sin ψ − c ω cos ψ 1 1 1 1 ....  y  =  sin ψ c1 cos ψ 0   a2  +  2b1 ω cos ψ − c1 ω 2 sin ψ  z¨ 0 0 1 a3 0 2 34 5 2 34 5 A(x,c1 )

b(x,b1 ,c1 )

In order to have a feedback system of the form:  ....    x v1 ....  y  =  v2  z¨ v3

We will take:

a = A−1 (x, c1 ) (v − b(x, b1 , c1 ))  −1 cos ψ −c1 sin ψ 0 0 =  sin ψ c1 cos ψ 0 0  1  v1 −2b1 ω sin ψ − c1 ω 2 cos ψ ×  v2  −  2b1 ω cos ψ − c1 ω 2 sin ψ  v3  0  2   cos ψ sin ψ 0 ω c1 v1 cos ψ ψ   v2  +  −2ωb1  0 =  − sin c1 c1 c1 v3 0 0 0 1

[2.22]

Feedback Linearization

119

5) By observing that b˙ 1 = a1 and u2 = a2 , we deduce that the state equations of our dynamic linearizer are:  c˙1 = b1    2  b˙ 1    = v1 cos ψ + v2 sin ψ + ω c1  c1 [2.23] u1    u2  =  1 (v2 cos ψ − v1 sin ψ − 2ωb1 )   c1   u3 v3 This linearizing feedback has v as input vector, u as output and (c1 , b1 ) as state vector. In order to have all the poles at −1, we choose (see [2.2]):  ... ... y 1 ) + .... w ¨1 − y¨1 ) + 4 (w 1 − ...  v1 = (w1 − y1 ) + 4 (w˙ 1 − y˙ 1 ) + 6 (w ... ....1 v2 = (w2 − y2 ) + 4 (w˙ 2 − y˙ 2 ) + 6 (w ¨2 − y¨2 ) + 4 (w 2 − y 2 ) + w 2  v3 = (w3 − y3 ) + 2 (w˙ 3 − y˙ 3 ) + w ¨3

Thus    v1 = (xd − x)    v2 = (yd − y)      v3 = (zd − z)

+4 (x˙ d − vx ) + 6 (¨ xd − c1 cos ψ) ... .... +4 ( x d − (b1 cos ψ − c1 ω sin ψ)) + x d +4 (y... ˙ d − vy ) + 6 (¨ yd − c1 sin ψ) .... +4 ( y d − (b1 sin ψ + c1 ω cos ψ)) + y d +2 (z˙d − vz ) + z¨d

[2.24]

where (xd , yd , zd ) corresponds to the desired trajectory for the center of the robot. By combining equations [2.23] and [2.24], we obtain the requested controller, which is of the form: ! x˙ r = fr (x, xr , t) u = gr (x, xr , t) where x = (x, y, z, ψ, vx , vy , vz , ω) is the state vector of the robot and where xr = (c1 , b1 ) is the state vector of the controller. The latter corresponds to two integrators added in front of the system, with the aim of eliminating the singularity in the linearization. The approach presented in this example is called dynamic feedback linearization. Indeed, the fact of having added integrators forces a state representation for our controller, in contrast with the situation without singularity which leads to a static relation for our controller. C ORRECTION FOR E XERCISE 2.8.– (Sliding mode control of a cart) 1) If we set y = (x1 , x2 ), we have !

y¨1 = −x˙ 3 x4 sin x3 + x˙ 4 cos x3 = −x4 sin x3 u1 + cos x3 u2 y¨2 = x˙ 3 x4 cos x3 + x˙ 4 sin x3 = x4 cos x3 u1 + sin x3 u2

120

Mobile Robotics

i.e. "

u1 u2

#

=

" 2

−x4 sin x3 x4 cos x3

cos x3 sin x3 34

φ(x,¨ y)

To have the error dynamic

#−1 "

# y¨1 . y¨2 5

yd (t) − y (t) + 2 (y˙ d (t) − y˙ (t)) + (¨ yd (t) − y ¨ (t)) = 0, we should take y ¨ (t) = yd (t) − y (t) + 2 (y˙ d (t) − y˙ (t)) + y ¨d (t). The corresponding controller is thus u =

"

−x4 sin x3 cos x3 x4 cos x3 sin x3

#−1

· (yd (t) − y (t) + 2 (y˙ d (t) − y˙ (t)) + y ¨d (t)) . 2) Take as the desired surface: s (x, t) = yd (t) − y (t) + y˙ d (t) − y˙ (t) = 0, which also corresponds to the dynamic of the error we want. Therefore " # " # " # cos t − sin(t) x4 cos x3 s (x, t) = 10 · − y (t) + 10 · − . sin (3t) 3 cos(3t) x4 sin x3 The controller is u = φ (x, K · sign (s (x, t))) where K is large, e.g. K = 100. Here, since s (x, t) is a vector of R2 , the function sign has to be understood componentwise. C ORRECTION FOR E XERCISE 2.9.– (Group of robots) 1) As for the previous exercise, if we take as an output y = (x1 , x2 ), and if we apply a feedback linearization method, we get the controller "

#−1 −x4 sin x3 cos x3 u = x4 cos x3 sin x3 " " # " # # x1 x4 · cos(x3 ) · c (t) − + 2˙c (t) − 2 +¨ c (t) , x2 x4 · sin(x3 )

[2.25]

Feedback Linearization

121

where c(t) is the desired position. For the ith robot, we take c (t) =

"

cos(at+ 2iπ m ) sin(at+ 2iπ m )

#

, c˙ (t) =a ·

"

− sin(at + 2iπ m ) cos(at + 2iπ m )

#

¨ (t) = − a2 c(t). ,c

2) To get the right ellipse, for each c (t), we apply the transformation "

# " # cos θ − sin θ 20 + 15 · sin(at) 0 w (t) = · ·c (t) sin θ cos θ 0 20 2 34 5 2 34 5 =R

=D

where w(t) is the new desired position. To apply our controller, we also need the two first derivatives of w(t). We have ˙ · D · c, ˙ ·c+R w ˙ = R · D · c˙ + R · D where ˙ = D

"

15a · cos(at) 0 0 0

#

˙ =a· , and R

"

− sin θ − cos θ cos θ − sin θ

#

.

Moreover, ˙ · D · c˙ ¨ ·c+R ¨ ·D·c+2·R ¨+R·D w ¨ = R·D·c ˙ · c˙ + 2 · R ˙ ·D ˙ ·c +2 · R · D

where ¨ = D

"

−15a2 · sin(at) 0 0 0

#

¨ = −a2 · R. and R

We can now apply the controller given by equation [2.25], where c(t) is now replaced by w(t). Figure 2.26 illustrates the behavior of the control law and shows that the group moves exactly with the ellipse. C ORRECTION FOR E XERCISE 2.10.– (Convoy) 1) As for exercise 2.3, we can take: "

#−1 −va sin θa cos θa ua = va cos θa sin θa "" # " # " # " ## d x x ˆa xa ˆa va cos θa · − + − yˆa ya va sin θa dt yˆa

122

Mobile Robotics

30

30

20

20

10

10

0

0

10

10

20

20

30

30 40

20

0

20

40

30

30

20

20

10

10

0

0

10

10

20

20

30

40

20

0

20

40

40

20

0

20

40

40

20

0

20

40

40

20

0

20

40

30 40

20

0

20

40

30

30

20

20

10

10

0

0

10

10

20

20

30

30 40

20

0

20

40

30

30

20

20

10

10

0

0

10

10

20

20

30

30 40

20

0

20

40

Figure 2.26. During the transient phase, all robots go toward their setpoints in the ellipse. Then, the group exactly follows the deformation of the ellipse. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Feedback Linearization

123

where x ˆa = Lx sin ωt, yˆa = Ly cos ωt,

dˆ xa = ωL cos ωt x dt dˆ ya = − ωLy sin ωt. dt

2) We have the new state equation s˙ = v, which corresponds to the virtual odometer. Each time s is larger than ds = 0.1 m, we set s = 0 and we store the corresponding state of RA in a matrix S which is available to the followers. Since the robot has to be at a distance d · i from RA , it has to follow the position that RA had at a time t − δi , when it was at a distance d · i m earlier. Now, the speed to be followed is that of RA , at the current time t. Indeed, if RA slows down, all followers should also slow down immediately. Thus the control to be given at time t is "

#−1 "" # " # −vi sin θi cos θi xa (t − δi ) xi u(i) = · − vi cos " θi sin θi y (t − δ ) i ## yi # a" cos (θa (t − δi )) vi cos θi +va (t) · − sin (θa (t − δi )) vi sin θi where (xi , yi , θi , vi ) is the state of the ith robot at time t. The values (xa (t − δi ), ya (t − δi ), θa (t − δi )) have been stored in the matrix S. It was d·i ds steps earlier. Figure 2.27 illustrates the behavior of the control law and shows that the distance between two robots does not depend on the position of the robots on the ellipse. C ORRECTION FOR E XERCISE 2.11.– (Profiling float) 1) Since the mass of the float is m = (1 + βb) ρ0 *3 , from Newton’s second law, we have 1 mv˙ = mg − ρ0 g*2 · max (0, * + min(d, 0)) − ρ0 v· | v | *2 cx . 2 The state equations are thus  ˙  d = v˙ = g −  ˙ b =

v g·max(0,)+min(d,0))+ 12 v·|v|cx (1+βb))

u

2) When no saturation exists (the float is fully immersed), we have  ˙  d = v˙ = g −  ˙ b =

v g·)+ 12 v·|v|cx (1+βb))

u

124

Mobile Robotics 10

10

5

5

0

0

5

5

10

20

10

0

10

20

10

10

10

5

5

0

0

5

5

10

10

20

10

0

10

20

10

10

5

5

0

0

5

5

10

20

10

0

10

20

10

10

10

5

5

0

0

5

5

10

20

10

0

10

20

10

20

10

0

10

20

20

10

0

10

20

20

10

0

10

20

20

10

0

10

20

Figure 2.27. After the transient phase, all robots follow the leader with a constant separation distance between two robots. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

To use a sliding mode control, we need to compute the two first derivatives of the output y = d. We get y˙ = d˙ = v y¨ = g −

g·)+ 12 v·|v|cx (1+βb))

Since here, y¨ is monotonic with respect to b, to increase y¨, we increase b by setting u = umax = 1, and to decrease y¨, we choose u = −umax = −1. The sliding surface is s (x, t) = y¨d − y¨ + α1 (y˙ d − y) ˙ + α0 (yd − y) = 0. 2 34 5 2 34 5 2 34 5 e¨

e˙

e

Feedback Linearization

125

To have the stability, we can take α1 = 2,α0 = 1. Thus, on the sliding surface, we have: " # . g · * + 12 v· | v | cx ¨ s (x, t) = d0 − g − + 2 d˙0 − v + (d0 − d) = 0. (1 + βb) * 2 34 5 y¨

Since we are never exactly on the sliding surface, we need to control the value of y¨ to converge to the sliding surface. Thus, we choose . . . g·)+ 12 v·|v|cx u = sign d¨0 − g − (1+βb)) + 2 d˙0 − v + d0 − d . The state space transformation is thus     d y  v  y˙  = h (x) =   . g·)+ 12 v·|v|cx y¨ g − (1+βb)) The function h is bijective.

For a stabilization at a given depth d¯0 , we take yd (t) = d¯0 and y˙ d = y¨d = 0. The variables y(t) will converge to d¯0 . We have: / 0 h (x) → d¯0 , 0, 0 . Thus, d → 0, v → 0 and g −

g· (1+βb)

→ 0, or equivalently, b → 0.

Figure 2.28 shows a simulation in the case where d0 (t) = 5 m. At time t = 0, the float is left at the surface of the water without any immersed volume. 3) We have y˙ = d˙ = v g·)+ 1 v·|v|c

x 2 y¨ = g − (1+βb)) ... (|v|cx v)(1+βb))− ˙ (g·)+ 12 v·|v|cx )(β b˙ )) y = − (1+βb) / 2 )2 1 0 g · * + 2 v· | v | cx β (| v | cx v) ˙ = − + u 2 (1 + βb) * (1 + βb) * 2 34 5 2 34 5

b(x)

a(x)

Thus, we can take

" # 2 ... ... (| v | cx v) (1 + βb) * ˙ y 0 u = a−1 (x) ( y − b(x)) = / · − (1 + βb) * g · * + 12 v· | v | cx β

126

Mobile Robotics

with ... ... y = w + 3 · (w ¨ − y¨) + 3 · (w˙ − y) ˙ + w − y.

Figure 2.28. Representation of the position of the float with respect to the time t ∈ [0, 8] when the required depth is d0 = 5 m

C ORRECTION FOR E XERCISE 2.12.– (Field following) 1) We have y˙ = u +

sin x3 . 1 + x22

Assume that we want y˙ = −y. A feedback linearization-based control is x3 x3 u = y˙ − sin = −y − sin 1+x22 1+x22 x3 = −x3 − atanx2 − sin 1+x2 2

Note that we do not have any singularity. 2) Since y˙ = −y, we quickly get y = 0. In such a case, !

x˙ 1 = cos x3 = cos (−atan(x2 )) x˙ 2 = sin x3 = sin (−atan(x2 ))

The associated vector field is attracted by the line x2 = 0, as illustrated by Figure 2.29.

Feedback Linearization

Figure 2.29. Precise line following

3) We take y = x3 − atan2(−(0.01 x21 − 1)x2 − x1 , x2 ). 2 34 5 2345 a

b

We have

b a ˙ y˙ = x˙ 3 − (− 2 · a˙ + 2 · b) a + b2 a + b2 2 34 5 2 34 5 ∂atan2(b,a) ∂a

=

u+

b·a−a· ˙ b˙ a2 +b2

∂atan2(b,a) ∂b

with a a˙ b b˙

= = = = =

x2 sin/x3 0 − 0.01 x21 − 1 x2 −/x1 0 2 − (0.02 · x1 x˙ 1 ) x2 − 0.01 / x1 2− 1 0x˙ 2 − x˙ 1 −0.02 · x1 x2 cos x3 − 0.01 x1 − 1 sin x3 − cos x3

Therefore, a feedback linearization-based control is u = y˙ −

˙ b˙ ) (b·a−a· a2 +b2

= −y −

= − (x3 − atan2(b, a)) −

˙ b˙ ) (b·a−a· a2 +b2

˙ b˙ ) (b·a−a· a2 +b2

.

127

128

Mobile Robotics

Moreover, to avoid the discontinuity on the error angle, we have to use the sawtooth function (see section 3.1). The final controller is thus - - 2 . .. x1 u = −sawtooth x3 − atan2 − 100 − 1 x2 − x1 , x 2 +

''

x2 1 100 −1

(

( ' ' 2 ( ( x1 x1 x2 cos x3 x2 +x1 ·sin x3 +x2 · + 100 −1 sin x3 +cos x3 50 '' 2 ( (2 x1 x22 + 100 −1 x2 +x1

The behavior of the controller is illustrated by Figure 2.30.

Figure 2.30. Robot accurately describing the Van der Pol cycle

C ORRECTION FOR E XERCISE 2.13.– (Sliding pendulum) 1) We have: y = x1 y˙ = x2 y¨ = − sin x1 + u. If we take u = sin x1 + (w − y + 2 (w˙ − y) ˙ + w) ¨ , 2 34 5 φ(x,(w−y+2(w−y)+ ˙ w)) ¨

we obtain

e + 2e˙ + e¨ = 0,

Feedback Linearization

129

where e = w − y is the error. Since w(t) = 0, ∀t, the controlled system is described by the following state equation: "

x˙ 1 x˙ 2

#

=

"

x2 −x1 − 2x2

#

.

The corresponding vector field is represented in Figure 2.31, on the right.

Figure 2.31. Left: vector field of the pendulum for u = 0. Right: system controlled with a feedback linearization method

2) We have: y = x1 y˙ = x2 If we are able to stay on the surface, s (x, t) = (w˙ − y) ˙ + (w − y) = 0, then the error e = w − y converges to zero. The sliding mode controller is u = sin x1 + K · sign ((w˙ − y) ˙ + (w − y)). 2 34 5 φ(x,K·sign(s(x,t)))

Since w(t) = 0, ∀t, the controlled system is described by the following state equation: "

x˙ 1 x˙ 2

#

=

"

x2 −K · sign (x1 + x2 )

#

.

130

Mobile Robotics

The corresponding vector field is represented on Figure 2.31 for K = 1 and K = 10. The sliding surface is also represented in blue. Note that for K = 1, the sliding surface is not attractive enough and thus some oscillation may occur in the neighborhood of the equilibrium.

Figure 2.32. Left: Vector field for the pendulum controlled with a sliding mode controller with K = 1. Right: The same for K = 10

3) We have V˙ (x) = = = = = =

s (x, t) · s˙ (x, t) ((w˙ − x2 ) + (w − x1 )) ((w˙ − x˙ 2 ) + (w ¨ − x˙ 1 )) (x2 + x1 ) (x˙ 2 + x˙ 1 ) (x2 + x1 ) (− sin x1 + u + x2 ) (x2 + x1 ) (K · sign (s (x, t)) + x2 ) (x2 + x1 ) (−K · sign (x2 + x1 ) + x2 ) .

We define the set SK = {x | (x2 + x1 ) (−K · sign (x2 + x1 ) + x2 ) > 0} . Since for all K, the set SK is never empty, the distance to the surface can always increase for some value of x and we are not able to prove the Lyapunov stability of the surface. Figure 2.33 depicts the set SK for some values of K.

Feedback Linearization

Figure 2.33. Sets SK for K = 1, 5, 10 in red. When K is large, the probability to go toward the sliding surface increases. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

131

3 Model-free Control

When we implement a controller for a robot and perform the initial tests we rarely succeed on the first try, which leads us to the problem of debugging. It might be that the compass is subject to electromagnetic disturbances, that it is placed upside-down, that there is a unit conversion problem in the sensors, that the motors are saturated or that there is a sign problem in the equations of the controller. The problem of debugging is a complex one and it is wise to respect the continuity principle: each step in the construction of the robot must be of reasonable size and has to be validated before pursuing construction. Thus, for a robot, it is desirable to implement a simple intuitive controller that is easy to debug before setting up a more advanced one. This principle cannot always be applied. However, if we have a good a priori understanding of the control law to apply, then such a continuity principle can be followed. Among mobile robots for which a pragmatic controller can be imagined, we can distinguish at least two subclasses: – vehicle-robots: these are systems built by man to be controlled by man such as the bicycle, the sailboat, the car, etc. We will try to copy the control law used by humans and transform it into an algorithm; – biomimetic robots: these robots are inspired by the movement of human beings. We have been able to observe them for long periods of time and deduce the strategy developed by nature to design its control law. This is the biomimetic approach (see, for example, [BOY 06]). We do not include walking robots in this category because, even though we all know how to walk, it is near to impossible to know which control law we use for it. Thus, designing a control law for walking robots [CHE 08] cannot be done without a complete mechanical modeling of walking and without using any theoretical automatic control methods such as those evoked in Chapter 2. For these two classes, we often do not have simple and reliable models available (this is the case, for example, of the sailboat or the bicycle). However, the strong understanding we have of them will allow us to build a robust control law.

Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

134

Mobile Robotics

The aim of this chapter is to show, using several examples, how to design such control laws. These will be referred to as mimetic control (we are trying to imitate humans or animals) or model-free control (we do not use the state equations of the robot to design the controller). Although model-free approaches have been largely explored in theory (see, for instance, [FLI 13]), here we will use the intuition we have of the functioning of our robot as much as possible. 3.1. Model-free control of a robot cart In order to illustrate the principle of model-free control, let us consider the case of a robot cart described by the equations:  x˙ = v cos θ    y˙ = v sin θ θ˙ = u2    v˙ = u1 − v

This model can be used for simulation, but not for obtaining the controller. 3.1.1. Proportional heading and speed controller We will now propose a simple controller for this system by using our intuition about the system. Let us take θ@ = θd −θ where θd is the desired heading and v@ = vd −v where vd is the desired speed. – For speed control, we take: u1 = a1 tanh v@

where a1 is a constant representing the maximum acceleration (in absolute value) that the motor is able to deliver. The hyperbolic tangent tanh (see Figure 3.1) is used as saturation function. Let us recall that: tanh x =

ex − e−x ex + e−x

[3.1]

– For the heading control, we take: @ u2 = a2 · sawtooth(θ)

In this last formula, sawtooth corresponds to the sawtooth function defined by: . @ = 2atan tan θ) = mod(θ@ + π, 2π) − π sawtooth(θ) [3.2] 2

Model-free Control

135

Figure 3.1. Hyperbolic tangent function used as saturation function

Let us note that, for numerical reasons, it is preferable to use the expression containing the modulus function. As illustrated in Figure 3.2, the function corresponds to an error in heading. The interest in taking an error θ@ filtered by the sawtooth function is to avoid the problem of the 2kπ modulus: we would like a 2kπ to be considered non-zero.

Figure 3.2. Sawtooth function used to avoid the jumps in the heading control

We may summarize this controller by: "

u1 u2

#

=

"

a1 · tanh (vd − v) a2 · sawtooth (θd − θ)

#

Thus, we will perform heading control. This model-free control, which works very well in practice, does not need to use the state equations of the robot. It is based on

136

Mobile Robotics

the understanding that we have of the dynamics of the system and recalls our wireless operation method of the cart robot. It has two parameters a1 and a2 that are easy to set (a1 represents the propelling power and a2 the directional disturbances). Finally, this controller is easy to implement and debug. N OTE.– In the three dimensional case, the heading of the robot θ and the desired heading θd are replaced by the orientation matrix R of the robot and the desired orientation Rd . The control u is replaced by a rotation vector uω the robot has to follow. The sawtooth control u = k · sawtooth (θd − θ) is replaced by (see equation [1.6]: uω = − where

. kα @ −R @T · Ad−1 R 2 sin α

α = acos @ = R

>

@ −1 tr(R) 2

?

RTd · R

3.1.2. Proportional-derivative heading controller For many robots, a proportional controller creates oscillations and it might prove to be necessary to add an damping or derivative term. This is the case for underwater exploration robots (of the type ROV (Remotely Operated Vehicle)) which are meant to stabilize above the zone of interest. Underwater torpedo robots do not have this oscillation problem given their control surfaces that stabilize the heading while in movement. If the heading if constant, such a proportional-derivative controller is given by: u2 = a2 · sawtooth (θd − θ) + b2 θ˙ ˙ it is generally The quantity θ may be obtained by a compass, for example. As for θ, obtained by a gyro. Low-cost robots do not always have a gyro available and we must try to approximate θ˙ from measurements of θ. However, a compass might jump by 2π for small variations in heading. This is the case, for instance, when a compass returns an angle within the interval [−π, π] and the heading varies around (2k + 1) π. In this case, an approximation of θ˙ must be obtained and this approximation has to be insensitive to these jumps. Let us denote by: Rt =

"

cos θ (t) − sin θ (t) sin θ (t) cos θ (t)

#

Model-free Control

137

the rotation matrix corresponding to the heading θ (t) of the robot (let us note that this matrix is insensitive to jumps of 2kπ). We have: ˙t RTt R

=

"

0 −θ˙ (t) θ˙ (t) 0

#

.

This relation can be seen as a two-dimensional (2D) version of relation [1.1] in section 1.1.4. Thus, an Euler integration of the rotation matrix: ˙t Rt+dt = Rt + dtR

# 0 −θ˙ (t) θ˙ (t) 0 " " ## 0 −θ˙ (t) = Rt I + dt ˙ θ (t) 0

= Rt + dt · Rt

"

Therefore: dt

-

0 −θ˙ (t) ˙θ (t) 0

.

= RTt Rt+dt − I . cos (θ (t + dt) −θ (t)) − sin (θ (t + dt) − θ (t)) = sin (θ (t.+ dt) −θ (t)) cos (θ (t + dt) − θ (t)) 1 0 − . 0 1

Let us take in this matrix equation the scalar equation corresponding to the second row and first column. We obtain: sin (θ (t + dt) − θ (t)) θ˙ (t) = . dt The proportional-derivative heading controller can, therefore, be written as: u2 (t) = a2 · sawtooth (θd − θ (t)) + b2

sin (θ (t) − θ (t − dt)) dt

which will be insensitive to jumps of 2π. 3.2. Skate car Let us consider the skating vehicle [JAU 10] represented in Figure 3.3. This vehicle that we will refer to as a skate car is purely imaginary. It is designed such that it moves on a frozen lake and stands on five ice skates. This system has two

138

Mobile Robotics

inputs: the tangent u1 of the angle β of the front skate (we have chosen the tangent as input in order to avoid the singularities) and u2 the torque exerted at the articulation between the two carts and corresponding to the angle δ. The thrust, therefore, comes only from the torque u2 and recalls the propulsion mode of a snake or an eel [BOY 06]. Any control over u1 will, therefore, not bring any energy to the system, but indirectly participates in the propulsion by generating waves. In this paragraph, we will propose a model in the form of a state for simulating the system. Concerning the control law, the existing general methods cannot deal with this kind of system and it is necessary to take into account the physics of the problem. Therefore we will propose a mimetic control law that allows us to obtain an efficient controller.

Figure 3.3. Skating robot moving like a snake

3.2.1. Model Let us try to obtain state equations capable of representing the dynamics of the system in order to simulate our system. The state variables are chosen to be x = (x, y, θ, v, δ), where x, y, θ correspond to the position of the front cart, v represents the speed of the center of the front sled axle and δ is the angle between the two carts. The angular speed of the front sled is given by: v1 sin β θ˙ = L1

[3.3]

where v1 is the speed of the front skate and L1 is the distance between the front skate and the center of the front sled axle. However: v = v1 cos β and therefore: v tan β vu1 = . θ˙ = L1 L1

[3.4]

Model-free Control

139

Viewed from the rear sled, everything is as if there was a virtual skate in the middle of the front sled axle, moving together with it. Thus, by recalling formula [3.3], the angular speed of the rear sled is: v sin δ θ˙ + δ˙ = − L2 where L2 is the distance between the centers of the axles, and therefore: v sin δ v sin δ vu1 [3.4] δ˙ = − − θ˙ = − − . L2 L2 L1

[3.5]

Following the theorem of kinetic energy, the temporal derivative of kinetic energy is equal to the sum of the powers supplied to the system: " # d 1 2 mv = u2 · δ˙ − (αv) · v [3.6] 2 34 5 2 34 5 dt 2 engine power

dissipated power

where α is the coefficient of viscous friction. For reasons of simplicity, we will assume here that the force of friction is equal to αv, which is the same as assuming that only the front sled is braking. We therefore have: " # v sin δ vu1 [3.6] 2 [3.5] ˙ mv v˙ = u2 · δ − αv = u2 · − − − αv 2 L2 L1 and

" # sin δ u1 mv˙ = u2 · − − − αv. L2 L1

The system can be described by the following state equations:  x˙ = v cos θ     y˙ = v sin θ   [3.4] θ˙ = vu1  [3.7]   v˙ = − (u1 + sin δ) u2 − v    ˙ [3.5] δ = −v (u1 + sin δ)

[3.7]

[3.8]

where, for reasons of simplicity, the coefficients (mass m, coefficient of viscous friction α, inter-axle distances L1 , L2 , etc.) have been given unit values. This system could be made control-affine (see equation [2.6]) by adding an integrator in front of u1 , however, the feedback linearization method cannot be applied due to the numerous singularities. Indeed, it can be easily shown that when the speed v is zero (easy to avoid) or when δ˙ = 0 (which necessarily happens regularly), we have a singularity. A biomimetic controller that imitates the propulsion of the snake or the eel might be feasible.

140

Mobile Robotics

3.2.2. Sinusoidal control By trying to imitate the control strategy of an undulating snake’s movement, we choose u1 of the form: u1 = p1 cos (p2 t) + p3 where p1 is the amplitude, p2 the pulse and p3 the bias. We choose u2 such that the ˙ 2 ≥ 0. Indeed, δu ˙ 2 corresponds to the propelling torque is a motor torque, i.e. δu power supplied to the robot that is transformed into kinetic energy. If u2 is bounded by the interval [−p4 , p4 ], we choose a bang-bang type controller for u2 of the form: ˙ u2 = p4 · sign(δ) which is equivalent to exerting maximum propulsion. The chosen state feedback controller is therefore: " # p1 cos (p2 t) + p3 . u= p4 sign (−v (u1 + sin δ)) The parameters of the controller remain to be determined. The bias parameter p3 allows it to direct its heading. The power of the motor torque gives us p4 . The parameter p1 is directly linked to the amplitude of the oscillation created during movement. Finally, the parameter p2 gives the frequency of the oscillations. The simulations can help us to set the parameters p1 and p2 correctly. Figure 3.4 illustrates two simulations in which the robot begins with an almost zero speed. In the simulation on top, the bias p3 is equal to zero. In the bottom simulation, p3 > 0. Figure 3.5 represents the advance as a function of time. It is clear that the power supplied by the engine is very strong at startup whereas in cruising regime, it is underutilized. Such a controller forces us to oversize our engine. It would be to our advantage to have a thrust as constant as possible. 3.2.3. Maximum thrust control The propulsion of the robot is done by the thrust u2 · δ˙ = −v (u1 + sin δ) · u2 and therefore by the engine that generates the torque u2 . In order to move as fast as possible, for a given motor, the engine should supply a maximum amount of power denoted by p¯ which will be transformed into kinetic energy. Thus: −v (u1 + sin δ) · u2 = p¯. 2 34 5 δ˙

Model-free Control

141

Figure 3.4. Various simulations illustrate the control law for the skating robot

Figure 3.5. Thrust supplied by the engine u2

There are therefore several torques (u1 , u2 ) capable of supplying the desired power p¯. We will therefore choose for u2 the form: u2 = ε · u ¯2 with ε = ±1 where ε (t) is a square wave and u ¯2 is a constant. This choice for u2 may be bound to the engine torque and thereby limit the mechanical load. If we choose the frequency of ε too low, the power supplied will be respected, but the front cart will collide with the rear cart. In the borderline case where ε is constant, we can observe, through the

142

Mobile Robotics

simulation, the first cart roll up to the second (which means that δ increases to infinity). We obtain, by isolating the orientation u1 of the front skate: " # p¯ u1 = − + sin δ . vε¯ u2 The maximum thrust controller is, therefore, given by: #  " p¯ − + sin δ . u= vε¯ u2 ε¯ u2

[3.9]

Thus, with this controller, not only do we always thrust in the correct direction through u2 but we can also adjust the direction u1 in order for the torque supplied by u2 to translate into a maximum thrust p¯. Now, we only need to act on ε (which, as we recall, is a square wave equal to ±1) and on the power p¯. The duty cycle of the signal ε (t) will allow us to direct our orientation and its frequency will give us the amplitude of the oscillations for the robot’s path. As for u ¯2 , it allows us to control the average speed of the robot. In the simulation, this controller turns out indeed to be more efficient than the sinusoidal controller. Figure 3.6 shows the angle of the skate β as a function of time, once the cruising regime has been reached. Let us note that the angle of the front skate β =atan(u1 ) makes discontinuities appear.

Figure 3.6. Evolution of the front skate angle β in cruising regime

3.2.4. Simplification of the fast dynamics The state equations for the skate car contain numerous singularities and we would like to simplify them here. However, in our system, we have two interfering dynamics:

Model-free Control

143

one that is slow (representing the smooth evolution of the state variables) and one that is fast (rated by ε) which creates the undulation. The idea, relatively standard in automatic control, is to average these values in a way to make the fast dynamics disappear. We can find this idea in PWM-controlled (Pulse Width Modulation) DC engines. Let us consider a high-frequency square wave signal ε (t). Its temporal average ε¯ is called the duty cycle. This duty cycle is set to vary very slowly in time. The temporal average operator is linear (just like the mathematical expectation). For example: 2ε1 (t) − 3ε2 (t) = 2¯ ε1 (t) − 3¯ ε2 (t) . On the other hand, for a nonlinear function f , we cannot write f (ε) = f (¯ ε). For instance, ε−1 &= ε¯−1 . However, we will have ε−1 = ε¯ if ε (t) ∈ {−1, 1}. This comes from the fact that the signals ε and ε−1 are equal in such a case. If a (t) and b (t) are slowly varying signals in time, we will also have: a (t) ε1 (t) + b (t) ε2 (t) = a (t) ε¯1 (t) + b (t) ε¯2 (t) . We will try to apply these approximations to the case of the skate car in order to eliminate the fast dynamics. By recalling the state equations in [3.8] and by injecting control law [3.9], we obtain a feedback system described by the following state equations:  x˙ = v cos θ    y ˙ = v sin θ     p¯   θ˙ = − − v sin δ ε¯ u2 p¯   v˙ = − v   v      δ˙ = p¯ ε¯ u2

Recall that we can act on the constants p¯, u ¯2 and on the square wave signal ε = ±1 that we will here consider to be high-frequency and with a duty cycle of ε¯. We can approximate: p¯ ε¯ p = ε¯ u2 u ¯2

(by linearization)

.

The system thus becomes:  x˙ = v cos θ     y ˙ = v sin θ   ˙ θ = −¯ pq¯ − v sin δ p¯   v˙ = − v   v   ˙ δ = p¯q¯

ε¯

p¯ . u ¯2

144

Mobile Robotics

which now only has two inputs: p¯ and q¯ = u¯ε¯2 . Let us try to control the inputs θ and v by using the feedback linearization method. Note that although this system is not affine in its inputs (¯ p and q¯), the method can be applied because, as we will see below, the necessary inversion is possible here. For this, let us define two new inputs v1 , v2 such that: A v1 = −¯ pq¯ − v sin δ p¯ v2 = − v v By inverting this system relative to the inputs, we obtain: p¯ = v (v2 + v) v1 + v sin δ q¯ = − v (v2 + v) Thus, the feedback linearized system is: ! θ˙ = v1 v˙ = v2 A proportional controller is, therefore, sufficient. We will take one that places the poles at −1: ! v1 = w1 − θ + w˙ 1 v2 = w2 − v + w˙ 2 Let us summarize the control law in its entirety. Its setpoints are w1 , w2 which correspond to the desired heading and speed. It is given by the following table: p¯ = v (w2 + w˙ 2 ) q¯ = −

w1 − θ + w˙ 1 + v sin δ v (w2 + w˙ 2 )

ε¯ = q¯ · u ¯2 (choose ε¯ ∈ [−1, 1] )

ε : duty cycle ε¯ and frequency slot ∞ #  " p¯ − + sin δ  u= vε¯ u2 ε¯ u2

The adjustment parameter u ¯2 involved in this controller is quite delicate to set. We need to choose u ¯2 small enough to have ε¯ ∈ [−1, 1]. However, it must not be too close to zero in order for u1 to not be too large (which would cause too significant front skate movements). This variable u ¯2 influences the necessary distribution between the torque (through u2 ) and the movement (through u1 ) for generating power. Let us finally note that this latter control law, supposed to be more efficient, uses state equations of the system in its design and it is, therefore, difficult to call it model-free.

Model-free Control

145

3.3. Sailboat 3.3.1. Problem Let us recall the principles of model-free control and try to adapt it to a line-tracking controller for our sailboat. Here we will consider a sailboat whose sheet length is variable, but not directly the angle of the sail as was the case until now (see [2.11] in section 2.6). This robot has two inputs which are the angle of the rudder u1 = δr and the maximum angle of the sail u2 = δsmax (equivalently u2 corresponds to the length of the sheet). We will try to make the robot follow a line which passes through points a and b (see Figure 3.7).

Figure 3.7. Feedback control of the sailing robot

This problem is influenced by the control strategies of the VAIMOS robot [GOR 11] of I FREMER, the sailing boat of the E RWAN naval school and the Optimousse robot from the E NSTA-Bretagne (see Figure 3.8). As illustrated by Figure 3.9, we will denote by (x, y, θ) the posture of the boat, by v its advancing speed, by ω its angular speed, by fs the force of the wind on the sail, by fr the force of the water on the rudder, by δs the angle of the sail and by ψ the angle of the wind.

146

Mobile Robotics

Figure 3.8. Left: the VAIMOS sailing robot of I FREMER (in the background) and the Optimousse robot from the E NSTA Bretagne; right: robot of the École Navale (Naval School) following a line

Figure 3.9. Variables used in the state equations of the robot

3.3.2. Controller We will now try to find a controller that will enable the robot to track a line. The robot will be equipped with three sensors: a compass that gives us the heading θ, a weathervane that measures the angle of the wind ψ and a GPS that returns the position m of the boat. The robot will also be equipped with two actuators: a servo-motor that controls the angle of the rudder δr and a stepper motor that sets the length of the sheet and therefore the maximum angle δsmax of the sail (i.e. |δs | ≤ δsmax ). As for the

Model-free Control

147

controller, its setpoint is the line ab to track and it has a binary variable q ∈ {−1, 1} called the hysteresis which will be used for close hauled sailing. This controller will have few parameters which will also be easy to control. Among these parameters, we find the maximum rudder angle δrmax (typically δrmax = π4 ), the cutting distance r (i.e. we would like the distance to the line to always be smaller than r), the close haul angle ζ (typically ζ = π4 ), and the angle of the sail in crosswind β (typically β = 0.3 rad). We propose the following controller, taken from the article [JAU 12], that we will explain later:

1 2 3 4 5 6 7 8 9

Controller in: m, θ, ψ, a, b; out: δr , δsmax ; inout: q " # b−a e = det ,m − a -b − aif |e| > r then q = sign (e) ϕ = angle (b − a) e. ¯ θ = ϕ − atan / 0 r if cos ψ − θ¯ + cos ζ < 0 or (|e| − r < 0 and (cos(ψ − ϕ) + cos ζ < 0)) then θ¯ = π + ψ − qζ. δrmax ¯ δr = sawtooth(θ − θ) π π ( 2β ) > ? loglog(2) / 0 ¯ π cos ψ − θ + 1 δsmax = 2 2

The controller has a single state variable which is the binary variable q ∈ {−1, 1}. It is for this reason that it appears at the same time as input and output of the algorithm. Let us comment on this algorithm. Line 1 (calculation of the algebraic distance). We calculate the algebraic distance between the robot and its line. If e > 0, the robot is on the left of its line, and if e < 0, it is on the right. In the formula, the determinant is to be understood in the following way: det (u, v) = u1 v2 − v1 u2 . Line 2 (update of the hysteresis variable). When |e| > r, the robot is far from its line and the hysteresis variable q (that memorizes the starboard tack) is allowed to change value. If, for instance, e > r, then q will take the value 1 and will keep it until e < −r. Line 3 (calculation of the line angle). We calculate the angle ϕ of the line to track (see Figure 3.10). In the instruction, angle(u) represents the angle made by the vector u ∈ R2 relative to the Ox axis (towards the East).

148

Mobile Robotics

Figure 3.10. Nominal vector field that the robot tries to track, when possible

Line 4 (calculation of the nominal heading). We calculate the nominal angle θ¯ (see Figure 3.10), i.e. the one that we would like to have without worrying about the wind. We take: -e. θ¯ = ϕ − atan . r This expression for θ¯ translates to an attractive line. When e = ±∞, we have ¯ θ = ϕ ± π2 , which means that the robot has a heading that forms an angle of π2 with the line. For a distance e corresponding to the cutting distance r, i.e. e = ±r, we have θ¯ = ϕ ± π4 . Finally, on the line we have e = 0 and therefore θ¯ = ϕ, which corresponds to a heading with the direction of the line. As illustrated on Figure 3.11(a), some directions θ¯ may be incompatible with that of the wind. / 0 Line 5. When cos ψ − θ¯ + cos ζ < 0, the path θ¯ corresponds to a direction that is too close to the wind that the robot is incapable of following (see Figure 3.12). I The heading θ¯ is then impossible to keep. In this case, we need to switch to close haul mode, which means that the robot will do everything it can to face the wind, or more formally, the new direction becomes θ¯ = π + ψ ± ζ (see line 7). Figure 3.11(b) represents the corresponding vector field. The thin arrows correspond to the nominal field and the bold arrows represent the corrected field when necessary. In this representation, we have removed the hysteresis effect induced by the variable q (which is means that we always have q = sign(e)). Line 6 (keep close hauled strategy). This instruction implements the so-called keep close hauled strategy. If |e| < r or if cos(ψ − ϕ) + cos ζ < 0, then the boat is forced to move upwind, even when the heading θ¯ is admissible, for reasons of efficiency. This

Model-free Control

149

strategy is illustrated on Figure 3.11(c). In this figure, we have chosen a close haul angle of ζ = π3 (which corresponds to difficulties moving upwind) and given this, the line is considered to be against the wind.

Figure 3.11. a) The nominal vector field may be incompatible with the wind (here represented by the large arrow); b) vector field generated by the controller if we remove line 6. The thin arrows correspond to the nominal paths and the bold arrows correspond to the corrected paths; c) vector field generated by the controller with line 6 included

Figure 3.12. Some directions are not possible for the sailboat. These unfeasible directions form the no-go zone, represented in grey

150

Mobile Robotics

Line 7 (close hauled heading). The boat is in close haul and we choose θ¯ = π+ψ− qζ (the wind direction plus or minus the close haul angle ζ). The hysteresis variable q is forced to keep the same point of sail as long as the distance of r to the line is not reached. An illustration of the resulting behavior is represented in Figure 3.13. If the nominal heading can be kept, then it is followed.

Figure 3.13. Keep close hauled strategy by remaining within the strip centered on the line ab with diameter r

Line 8 (rudder control). At this level, the heading to maintain θ¯ has already been chosen and we are trying to follow it using the rudder. We perform a proportional ¯ In order to filter out the modulus-2π problem, we control relative to the error θ − θ. use the sawtooth function (see formula [3.2]). We thus obtain: δr =

δrmax ¯ · sawtooth(θ − θ) π

where δrmax is the maximum angle of the rudder (for example δrmax = 0.5 rad). The resulting controller is illustrated in Figure 3.14. Line 9 (sail control). We choose a sail angle β (half-open sail) that the sail needs to have in crosswind. This parameter is determined experimentally depending on the sailboat and the steering mode we would like to use. The maximum angle of the sail

Model-free Control

151

δsmax is a function of ψ − θ which is periodic with period 2π. One possible model [JAU 12] is that of the cardioid: δsmax =

π · 2

"

cos (ψ − θ) + 1 2

#η

where the parameter η is positive. When ψ = θ + π, the boat is facing the wind and the model gives us δsmax = 0. When ψ = θ, we have δsmax = π2 , which means that the sail is wide open when the robot is with the wind. The choice of the parameter η will be based on the angle of the sail in crosswind, i.e. for ψ = θ ± π2 . The equation δsmax = β for ψ = θ ± π2 is translated by: π · 2

" #η 1 =β 2

i.e.: log η=

-

π 2β

.

log (2)

Figure 3.14. Rudder control for the sailing robot

The function δsmax is represented in Figure 3.15. On the tests that have been carried out, this adjustment of the sail was shown to be efficient and easy to control, given the small number of parameters.

152

Mobile Robotics

Figure 3.15. Adjusting the maximum sail angle (or the sheet length); left: Cartesian representation; right: polar representation

3.3.3. Navigation Once the line tracking has been correctly implemented and validated, a number of lines should be chained together with the aim of performing complex missions (such as, for instance, connecting two points of the globe). In such a context, a Petri net strategy is well-adapted for representing the discrete state changes [MUR 89]. Figure 3.16 shows a Petri net allowing us to manage the mission. Before the robot is launched, it is in an initial state represented by the place p0 . The transition t1 is crossed at the start of the mission. If everything goes well, the robot is in state p1 and is tracking its first line a1 b1 . The line aj bj is validated as soon as the point bj is surpassed, i.e. if *bj − aj , m − bj + > 0. This stopping criterion coupled with the path can be interpreted as a sort of validation. Once this is validated, we proceed to the next line. When the list of lines to track is empty, the mission ends (place p3 ).

Figure 3.16. Petri net supervising the navigation of the robot

Model-free Control

153

Figure 3.17. Experiment of the spiral composed of five stages: a) the robot begins with a triangle (in the circle); b) it goes upwind following a line; c) it describes a spiral; d) it anchors virtually at the center of the spiral for several minutes; e) and goes back to the harbor

3.3.4. Experiment Beginning in September 2011, we carried out a series of experiments with the VAIMOS sailboat in autonomous mode. We will describe one of these experiments, which is simultaneously simple and representative, which took place on Thursday 28 June 2012 in the Brest bay, close to the Moulin Blanc harbor. The trajectory performed by the robot is represented in Figure 3.17. The wind comes from the South-South-East, as indicated by the arrow that represents the average wind on the robot during the entire mission, deduced from the sensor. In this zone of heavy maritime traffic, interruptions in the mission can be anticipated and a permanent WiFi link is necessary between the robot and the tracking boat in order to be able to cancel the mission at any time and avoid collision with other boats. Apart from these security interruptions (which, by the way, were not necessary during the mission), the robot is entirely autonomous. The mission is broken down into five sub-missions. First of all, the robot begins with a triangle (in the circle) in order to check whether everything is working properly. Then, it proceeds South-East against the wind by

154

Mobile Robotics

tracking the required path. It then describes a spiral. Once it is in the center of the spiral, the robot anchors virtually, i.e. it maneuvers in order to remain around its attachment point. Finally, the robot returns to the harbor with the wind. Other larger-scale experiments were also carried out, such as the journey from Brest to Douarnenez (see Figure 3.18) undertaken on the 17th–18th January 2012, thus completing a path of more than 100 km. From very high up (as in the figure), the lines seem to be tracked perfectly. Upon closer inspection, things are revealed to be less ideal: the sailboat tacks in order to go upwind, re-calibrates itself or is subjected to large waves otherwise. However, in both previously described experiments, the robot is never more than 50 meters away from its track (except of course in the situations of avoidance in which it is being hauled).

Figure 3.18. Journey from Brest to Douarnenez made by VAIMOS: a) the robot leaves the Moulin Blanc harbor (in the circle); b) it avoids a submarine (in the square); c) it avoids a cargo ship (triangle)

N OTE.– Even though the robot never surpasses its line by more than 50 m, we could do better and improve this precision when the robot follows the nominal heading (i.e. the angle ϕ of the line corresponds to a sustainable heading). Indeed, in our experiments, a 10 m bias can be observed in nominal mode, which means that the distance to the line does not converge towards zero (with GPS precision). The role of the integrator is to remove such a bias. In order to implement such an integrator, we simply replace line 4 of the controller with the following two instructions:  e  z = z + α dt " # e+z  θ = ϕ − atan r

Model-free Control

155

where dt is the sampling period. The variable z corresponds to the value of the integrator and naturally converges towards the constant bias that we had without the integrator and which we would like to remove. The coefficient α has to be sufficiently small to avoid a change in the behavior of our robot (which could appear in transient regime). For instance, if e = 10 m for 100 seconds, we may want a correction of 1 m of the bias. For this, we need to take α = 0.001. Let us note that as soon as the distance on the line is greater than r (this is the case, for instance, during initialization), when the robot validates a line and continues on to the next, or when the robot is in large mode, then the integrator has to be forced to zero. Indeed, an integrator must not take up its function unless the permanent regime has been established. 3.4. Exercises E XERCISE 3.1.– Robot tank on a line Let us consider a robot moving on a plane and described by the following state equations:   x˙ = cos θ y˙ = sin θ ˙ θ = u

where θ is the heading of the robot and (x, y) are the coordinates of its center. This model corresponds to the Dubins car [DUB 57]. The state vector is given by x = (x, y, θ). 1) Simulate this system graphically with in various situations. 2) Propose a heading controller for the robot. 3) Propose a controller tracking a line ab. This line must be attractive. Stop the T program when point b is overtaken, i.e. when (b − a) (b − m) < 0.

4) Make the robot track a closed path composed of a sequence of lines aj bj , j ∈ {1, . . . , jmax }.

5) Make several identical robots track the same circuit, but with different speeds. Modify the control laws in order to avoid the collisions. E XERCISE 3.2.– Van der Pol car Consider the car represented in Figure 3.19. This car has two controls: the acceleration of the front wheels and the rotation speed of the steering wheel. The state variables of our system are composed of the

156

Mobile Robotics

position coordinates (the coordinates x, y of the center of the rear axle, the heading θ of the car and the angle δ of the front wheels) and the speed v of the center of the front axle. The evolution equation of the car is assumed to be identical to that of a tricycle (see section 2.5.1). It is written as:    v cos δ cos θ  x˙ v cos δ sin θ   y˙       v sin δ   θ˙  =   .     3  v˙    u1 ˙δ u2

Figure 3.19. Car moving on a plane (view from above)

1) Simulate this system using a Euler’s method. 2) Perform a first high-gain proportional feedback u = ρ (x, u ¯ ) that would allow switching to a cart model of the form:     x˙ u ¯1 cos θ  y˙  =  u ¯1 sin θ  . ˙θ u ¯2 The new inputs of the feedback system u ¯1 and u ¯2 correspond respectively to the ˙ speed v and to the angular speed θ. 3) Perform a second feedback u ¯ = σ (x, w) that would allow us to control this car’s heading and speed. The new input will be w = (w1 , w2 ) where w1 , w2 correspond respectively to the desired speed and heading. 4) We would like the car to follow a path that obeys the Van der Pol equation: !

x˙ 1 = x2/ 0 x˙ 2 = − 0.01 x21 − 1 x2 − x1

Model-free Control

157

Figure 3.20. Car describing the Van der Pol cycle

Propose a third controller w = τ (x) that would allow us to perform this. Validate it using a simulation by superimposing the vector field as in Figure 3.20. E XERCISE 3.3.– Anchoring Let us consider the robot described by the following state equations:   x˙ = cos θ y˙ = sin θ ˙ θ = u

The aim of anchoring the robot is to remain within the neighborhood of zero. 1) Does this system have a point of equilibrium? 2) Given the fact that the problem of remaining around zero admits a rotational symmetry, we suggest switching from a Cartesian representation (x, y, θ) towards a polar representation (α, d, ϕ), as shown in Figure 3.21. Give the state equations in the polar representation.

158

Mobile Robotics

Figure 3.21. Coordinate system change allowing us to take advantage of the rotational symmetry

3) How is this new representation of interest for the graphical representation of the system dynamics? 4) In order to solve the anchoring problem we propose the control law: ! +1 if cos ϕ ≤ √12 (the robot turns to the left) u= − sin ϕ otherwise (proportional control) An illustration of this control law is given in Figure 3.22. Explain how this control solves the anchoring problem. Is there a configuration that allows the robot to move arbitrarily far away from 0? 5) Simulate this control law with various initial conditions.

Figure 3.22. Path of the controlled system in order to remain within the disk

Model-free Control

159

E XERCISE 3.4.– Discretization of the state space Let us consider the following system (which stems from the previous exercise):   sin ϕ     +1 if cos ϕ ≤ √12    d  # (i) ϕ˙ = " 1   − 1 sin ϕ otherwise    d    (ii) d˙ = − cos ϕ

The associated vector field is represented in Figure 3.23, where ϕ ∈ [π, π] and d ∈ [0, 10]. A path φ (t, x0 ) which corresponds to the simulation visualized in the previous exercise is also drawn.

Figure 3.23. Vector field associated with our system

In this same figure, the state space is cut into six areas. Indeed, given the adjunction of the line ϕ = π with the line ϕ = −π, the state space has a cylindrical nature and we have six areas, whereas we can see eight areas on the figure. Succession relation. We define the rotation, denoted by 2→ between zones A, B of the state space as follows: (A 2→ B) ⇔ ∃x0 ∈ A ∈ φ (η (x0 ) , x0 ) ∈ B where η (x0 ) is the time the system exits A.

160

Mobile Robotics

Convention. If there is x0 ∈ A, such that ∀t > 0, φ (t, x0 ) ⊂ A then η (x0 ) = ∞. Thus, φ (η (x0 ) , x0 ) ∈ A and therefore will we have (A 2→ A). 1) Draw the graph associated with this relation.

2) From this, deduce a superset of the state space in which the system will remain trapped. E XERCISE 3.5.– Sailing robot Consider the sailboat described by the following state equations:  x˙     y˙     θ˙        v˙         ω˙   fs   fr     σ       δs         wap      ψap

= v cos θ + p1 a cos ψ = v sin θ + p1 a sin ψ = ω fs sin δs − fr sin u1 − p2 v 2 = p9 fs (p6 − p7 cos δs ) − p8 fr cos u1 − p3 ωv = p10 = p4 -wap - sin (δs − ψap ) = p5 v sin u1 = cos ! ψap + cos u2 π + ψap if σ ≤ 0 = −sign (sin ψ ) · u otherwise ap 2# " a cos (ψ − θ) − v = a sin (ψ − θ) = angle wap

where (x, y, θ) corresponds to the posture of the boat, v is its forward speed, ω is its angular speed, fs (s for sail) is the force of the wind on the sail, fr (r for rudder) is the force of the water on the rudder, δs is the angle of the sail, a is the true wind speed, ψ is the true wind angle (see Figure 3.9) and wap is the apparent wind vector. The quantity σ is an indicator of the sheet tension. Thus, if σ ≤ 0, the sheet is released and the sail is flapping. If σ ≥ 0, the sheet is stretched and inflated by the wind. In these equations, the pi are design parameters of the sailboat. We will take the following values, given in international units: p1 = 0.1 (drift coefficient), p2 = 1 (drag coefficient), p3 = 6 000 (angular friction of the hull against the water), p4 = 1 000 (sail lift), p5 = 2 000 (rudder lift), p6 = 1 (position of the wind’s center of thrust on the sail), p7 = 1 (position of the mast), p8 = 2 (position of the rudder), p9 = 300 (mass of the sailboat) and p10 = 10 000 (inertial momentum of the sailboat). 1) Simulate the boat. 2) Implement the controller proposed in section 3.3. We will use the following parameters ζ = π4 for the large-angle, r = 10 m for the radius of the air corridor,

Model-free Control

δrmax = 1 rad for the maximum angle of the rudder and β = sail in crosswind.

π 4

161

for the angle of the

3) Study the singularities (or the discontinuities) of the controller. What may happen if cos(ψ − ϕ) + cos ζ = 0? E XERCISE 3.6.– Flying drone Consider a flying drone [BEA 12] such as the one represented on Figure 3.24(a). This is a 1 kg fully autonomous plane. One possible model to describe its dynamics, very strongly inspired by those of Faser Ultra Stick [KLE 06] in Figure 3.24(b), is given by: 

Reuler (ϕ, θ, ψ) · v



        1 tan θ sin ϕ tan θ cos ϕ          0 cos ϕ − sin ϕ  · ω  ϕ˙          cos ϕ sin ϕ   θ˙     0     cos θ cos θ  ψ˙                − sin θ u1             0 −ω∧v 9.81 · cos θ sin ϕ + fa +   =  v˙    cos θ cos ϕ 0                 2 -v5ω1 − ω3     −ω3 ω2 − (β + 2u3 + )        10 -v  ω˙     2     -v300ω2        ω3 ω1 − (1 + 20α − 2u + 30u + ) 3 2   100 -v-    2    ω1 ω2 -vu3 ω1 − 2ω3 + (β + + ) 10 10 2 2 -v

p˙



Figure 3.24. a) µ-S TIC plane made by the E NSTA Bretagne; b) Faser Ultra Stick plane from the University of Minnesota; c) graphical representation used for the simulation

162

Mobile Robotics

with:

# " # v3 v2 , β = asin v1 -v  − cos α cos β cos α sin β sin α 2 -v-  sin β cos β 0  fa = 500 − sin α cos β sin α sin β − cos α   10ω2 2  4 + (−0.3 + 10α + -v- + 2u3 + 0.3u2 ) + |u2 | + 3 |u3 |      10ω − 3ω   3 1 ·  −50β +   -v  2 10 + 500α + 400ω + 50u + 10u 3 2 &v& α = atan

"

In this model, all the quantities are given in international units. The vector p = (x, y, z) represents the position of the drone, with the z axis oriented towards the center of the Earth. The orientation of the drone is represented by the Euler angles (ϕ, θ, ψ) and the Euler matrix Reuler (ϕ, θ, ψ) is given by formula [1.9] in section 1.2.1. The vector v represents the speed of the drone expressed in its own coordinate system. The rotation vector of the plane is denoted here by ω. It is linked to the derivatives of the Euler angles by formula [1.12] in section 1.3. Let us note that formula [1.12] gives the first three equations of our state model. The angles α and β correspond to the angle of attack and the sideslip angle. The vector fa corresponds to the acceleration caused by the forces created by air. A simplified geometric view of the drone, given in Figure 3.24(c), shows a helix for propulsion and two ailerons for direction. The input vector u = (u1 , u2 , u3 ) involved in our state model contained the propulsive acceleration u1 ∈ [0, 10] (in ms−2 ), the sum u2 ∈ [−0.6, 0.6] (in radians) of the two aileron angles and u3 ∈ [−0.3, 0.3] (in radians) the differential between these two ailerons. 1) Simulate this drone. For the graphics, see Figure 3.24(c). 2) Propose a heading, elevation and speed control law. 3) We would like the robot to be positioned on a circle of radius r¯ = 100 m, centered around 0 at an altitude of 50 m and a speed of v¯ = 15 ms−1 . Give the control law and illustrate the associated behavior of the robot. E XERCISE 3.7.– Quadrotor We consider the quadrotor represented in Figure 3.25. Its dynamic model (see section 1.4.2) is described by the equations in Figure 3.26. Note that the graph is represented in a causal manner from the speed ωi of each motor, which can be tuned independently to the position p of the robot. This causal sequence will be used for the synthesis of the controller.

Model-free Control

163

Figure 3.25. The quadrotor to be controlled. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

The state variables are the position p of the robot, the Euler angles (ϕ, θ, ψ), the speed vr expressed in the robot frame and the rotation vector ω r also expressed in the robot frame. The inertia matrix will be taken as   10 0 0 I =  0 10 0  . 0 0 20

Moreover, we will take m = 10, g = 9.81, β = 2, d = 1 and * = 1 all expressed in the international metric system. To control this quadrotor, a feedback linearization method could be carried out, but the computation of the controller is not easy and assumes that the model corresponds to the true system. Here, we want to use a more pragmatic method, based on a backstepping technique [KHA 02]. This corresponds to a sequence of loops, each of which inverts one block in the causal chain. This will make the controller easier to develop and debug. / 0 1) Propose a feed-forward controller with the new input τ d0 , τ d1 , τ d2 , τ d3 which eliminates the effect of block (a). Here, the superscript d means desired since it is what we want for the τi .

2) Using a feedback linearization / 0approach, build a controller with a desired input ω dr and an output τ d1:3 = τ d1 , τ d2 , τ d3 , such that the vector ω r converges to ω dr . This loop will make it possible to for us eliminate block (b). / d d d0 d 3) Build a controller with a desired input / d d d 0ϕ , θ , ψ and an output ω r , such that the vector (ϕ, θ, ψ) converges to ϕ , θ , ψ .

164

Mobile Robotics

Figure 3.26. Dynamic of the quadrotor represented by a causal chain which links five blocks (a), (b), (c), (d) and (e)

/ 0 4) Add another control loop with an output ϕd , θd , ψ d , τ d0 , based on a vector field approach, so that the quadrotor follows a path that obeys the Van der Pol equation: ! x˙ 1 = x2/ 0 x˙ 2 = − 0.001 x21 − 1 x2 − x1

Model-free Control

165

The quadrotor should also stay at an altitude of 15 m and a speed of vd = 10 ms−1 . Moreover, we want the front of the robot to point forward. An illustration of what should be obtained is depicted in Figure 3.27. The Van der Pol cycle is colored in green.

Figure 3.27. Simulation of the quadrotor which follows the Van der Pol cycle. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

E XERCISE 3.8.– Isobath An isobath is an imaginary curve that connects all of the points that have the same depth h(x, y) below a water surface, i.e. an underwater level curve. Consider an underwater robot described by the state equation:  x˙ = cos ψ    y˙ = sin ψ z˙ = u1    ˙ ψ = u2

where (x, y, z) corresponds to the position of the robot and ψ is its heading angle. For the initial condition, we take x = 2, y = −1, z = −2, ψ = 0. The robot is able to measure its altitude y1 (distance from the seafloor) and the angle y2 of the gradient

166

Mobile Robotics

∇h(x, y) of h(x, y) in its own frame by using a sonar. Moreover, it is able to know its depth z3 by using a pressure sensor. Thus, its observation function is   y1 = z − h(x, y) y2 = angle(∇h(x, y)) − ψ  y3 = −z / 0 For instance, if y = 7, − π2 , 2 , the robot knows that it is following an isobath corresponding to −y1 − y3 = −7 − 2 = −9 m, at a depth of 2 m. This is illustrated in Figure 3.28.

Figure 3.28. Underwater robot that has to follow an isobath. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

1) Propose a controller of the form u = r (y), which makes the robot follow an isobath corresponding to h0 = −9 m at a depth of y 3 = 2 m.

2) Validate your controller by using a simulation. An illustration of what should be obtained is depicted in Figure 3.29. For the simulation, we will consider a seafloor described by h (x, y) = 2 · e−

(x+2)2 +(y+2)2 10

E XERCISE 3.9.– Satellite

+ 2 · e−

(x−2)2 +(y−2)2 10

− 10.

Consider a satellite with coordinates (x1 , x2 ) in orbit around a planet, as illustrated in Figure 3.30. 1) We assume that we can control the tangent acceleration of the satellite by using a variable u. Show that the state equations for the system may be written as  x˙ 1 = x3    x˙ =  x4  2   αx1 x˙ 3 = − : 3 + x3 u  x21 + x22   αx2    3 + x4 u  x˙ 4 = − : 2 x1 + x22

Model-free Control

Figure 3.29. Simulation of an underwater robot (blue) following an isobath. The surface shadow (gray) and the seafloor shadow (black) are also depicted. (See exercise 3.8). For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Figure 3.30. The planet (blue) at coordinates (0, 0) and the satellite (red) at coordinates (x1 , x2 ). For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

167

168

Mobile Robotics

2) For simplicity, we assume that α = 1. Propose a feedback linearization-based method to find a controller for u (x) such that the satellite converges to the circle of radius R. Conclude. 3) Propose a proportional and derivative controller which stabilizes both the potential energy and the kinetic energy corresponding to the desired circle. Illustrate the behavior of the control in a simulation. 3.5. Corrections C ORRECTION FOR E XERCISE 3.1.– (Robot tank on a line) 2) The heading controller is given by u=sawtooth(thetabar-x(3)); Using the sawtooth function (see Figure 3.2) allows us to filter the jumps of angle ±2π.

3) As for following the line, we combine a heading controller with a vector field controller. The field used in order to make the line attractive is θ¯ = ϕ−atan(e) where e is the algebraic difference on the line. The program is:   −30 # " #  −10  40 20  1 a := ; b := ; x :=   π ; dt = 0.1 −4 6 1 2 For t = 0 : dt : 10 . "

3 4 5 6

b−a e := det &b−a& , x1:2 − a ; ϕ := angle (b − a) ¯ θ := ϕ − atan (e) u := sawtooth(θ¯ − x3 ) x := x + dt · f (x, u)

C ORRECTION FOR E XERCISE 3.2.– (Van der Pol car) 1) The simulation is performed with the following script: 1 x := (0, 0.1, π6 , 2, 0.6)T 2 dt := 0.01; u = (0, 0)T 3 For t = 0 : dt : 10   x4 cos x5 cos x3  x4 cos x5 sin x3    x4 sin x5   4 x=x+  · dt   3   u1 u2

Model-free Control

169

2) We simply need to take: >

u = ρ (x, u ¯) = k u ¯−

>

v cos δ v sin δ 3

??

with large k. Thus, by following the same reasoning as for the operational amplifier, we obtain that if the feedback system is stable, we have "

u ¯1 u ¯2

#

.

>

v cos δ v sin δ 3

?

and the feedback system becomes the tank model. Note that here, the control made it possible to remove two integrators in our model which is now of order 3. For the simulation, we could take k = 10. 3) For u ¯1 , there is nothing to do. For u ¯2 , we could take a proportional heading controller. The controller corresponding to this second feedback is: u ¯ = σ (x, w) =

!

u ¯ 1 = w1 u ¯2 = a2 · sawtooth (w2 − θ)

with, for example, a2 = 3. 4) We add a third feedback given by: 

w = τ (x) = 

angle

"

v0 y 0 / − 0.01 x2 − 1 y − x

#

 

where v0 is the desired speed for the car and the angle function returns the angle of a vector. For the simulation corresponding to Figure 2.12, we took v0 = 10 ms−1 and the car follows the Van der Pol cycle. The approach is more general, more robust, but also less accurate than the feedback linearization method illustrated by exercise 2.12. C ORRECTION FOR E XERCISE 3.3.– (Anchoring) 1) Since x˙ 2 + y˙ 2 = 1, the robot cannot be stopped. 2) The required state equations are the following:      (i) (ii)     (iii)

sin ϕ ϕ˙ = +u d ˙ d = − cos ϕ sin ϕ α˙ = − d

170

Mobile Robotics

The proofs for (ii) and (iii) are direct. Let us now prove (i). Following Figure 3.31, we have ϕ − θ + α = π. After differentiating, we get sin ϕ ϕ˙ = −α˙ + θ˙ = + u. d

Figure 3.31. Using cylindrical symmetry to switch from an order 3 system to an order 2 system

3) This representation allows us to switch from a 3-dimensional system to a 2-dimensional one (since α is no longer necessary). A graphical representation of the vector field then becomes possible (see exercise 3.4). 4) The idea behind this control law is to use a proportional controller when the robot approximately points towards 0 (i.e. if cos ϕ > √12 ) and turn to the left otherwise, in order to point towards 0 later. C ORRECTION FOR E XERCISE 3.4.– (Discretization of the state space) The graph is represented in Figure 3.32 in two different forms. The state will be trapped under the bold line. In matrix form, the graph is given by:   0 1 0 1 0 0 0 0 0 0 1 0    1 1 0 0 1 1   . G= 1  0 0 0 0 1  0 0 0 0 0 1  0 0 0 1 0 [0, 1]

Model-free Control

171

where the Boolean interval [0, 1] means either 0 or 1. The transitive closure is:   0 1 0 1 1 1 0 0 0 1 1 1   1 1 0 1 1 1 + 2 3   G = G + G + G + ··· =   0 0 0 1 1 1 0 0 0 1 1 1 0 0 0 1 1 1

Figure 3.32. Discretization of the system’s dynamics by a partitioning of the state space in order to obtain a transition graph

The diagonal gives us the attractor of the graph X∞ = X4 ∪ X5 ∪ X6 . thus, the attractor of the system A satisfies: A ⊂ X4 ∪ X5 ∪ X6 C ORRECTION FOR E XERCISE 3.5.– (Sailing robot) 1) For the simulation, we can use a Euler method, by taking as evolution function: Function f (x, u) 1 (x, y, θ," v, ω) := x # a cos (ψ − θ) − v 2 wap := a sin (ψ − θ) 3 ψap := angle wap ; σ = cos ψap + cos u2 4 If σ ≤ 0 then δs := π + ψap else δs := −sign (sin ψap ) · u2 5 fr := p5 v sin u1 ; fs := p4 -wap - sin (δs − ψap )   v cos θ + p1 a cos ψ   v sin θ + p1 a sin ψ     ω   2 6 Return  fs sin δs − fr sin u1 − p2 v      p  f (p − p cos δ ) −9p f cos u − p ωv  s 6 7 s 8 r 1 3 p10

172

Mobile Robotics

Figure 3.33. Simulation for the line following of a sailboat. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

2) For line tracking, we write the controller as follows: Controller in:x; out:u; inout:q π 1 (x, y, θ, v,- ω) := x; m := . x1:2 ; r := 10; ζ := 4 b−a 2 e := det &b−a& , m − a ; ϕ := angle (b − a) 3 if |e| > r then/q := 0 sign (e) e 4 θ¯ := ϕ − atan / / 0r 0 5 if cos ψ − θ¯ + cos ζ < 0 6 or (|e| − r < 0 and (cos(ψ − ϕ) + cos ζ < 0)) 7 then θ¯ := π + ψ#− qζ " ¯ 0.3 · sawtooth(θ / 0 − θ) 0 8 u := π / ¯ 4 cos ψ − θ + 1

Model-free Control

173

Figure 3.35 shows a simulation of the controller for different lines. The wind always comes from the top (or North). In subfigures (b) and (c), the boat has to go upwind. It has to tack from q = 1 to q = −1 to stay inside the corridor of radius r.

3) Since ψ, ϕ, ζ are constant, / 0 when cos(ψ − ϕ) + cos ζ = 0 and the robot is on the line, we also have cos ψ − θ¯ + cos ζ = 0 at step 5. The controller may alternate indefinitely between two strategies: θ¯ := ϕ and θ¯ := π + ψ − qζ. This hesitation corresponds to a discontinuity of the controller that can be seen on simulations but also sometimes for short periods during experiments with actual sailboats. For a deeper understanding, take the special case where ϕ = 0 and we can characterize the discontinuities of our controller by computing the set S of all (e, ψ) such that the controller chooses the strategy θ := π + ψ − qζ. It is defined by S = S1 ∪ (S2 ∩ S3 ) where S1 = S2 = S3 =

C

- e .. D (e, ψ) | cos ψ + atan + cos ζ < 0 r {(e, ψ) | |e| − r < 0} {(e, ψ) | cos ψ + cos ζ < 0}

Figure 3.34. Set S where the strategy θ := π + ψ − qζ is chosen in the (e, ψ)-space. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Figure 3.34 illustrates the operation for ϕ = 0, r = 10, ζ = π4 , e ∈ [−12, 12], ψ ∈ [−π, −π]. The set of singularities contains the boundary of S as in shown in Figure 3.35. The single persistent singularities are represented by the two red points. C ORRECTION FOR E XERCISE 3.6.– (Flying drone) 2) For the control law, we need to control the propulsion by u1 , the elevation by u2 and the heading by u3 . Propulsion. On our drone, the propulsion must be within the interval [0, 10] (in ms−2 ). We will take: u1 = 5 (1 + tanh (¯ v − -v-))

174

Mobile Robotics

where v is the speed vector of the drone and v¯ is the speed setpoint. If the drone is moving at the correct speed, we have an average propulsion of 5 N. Otherwise, due to the hyperbolic tangent saturation function tanh(·) (refer to formula [3.1]), we will always have a propulsion within the interval [0, 10].

) * Figure 3.35. Set S of singularities of our controller in the e, ψ˜ -space. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Elevation. The elevation will be controlled using the sum of the aileron angles that we assume to be in the interval [−0.6, 0.6] rad. We will take: / / / 00 0 u2 = −0.3 · tanh 5 θ¯ − θ + |sin ϕ| .

Thus, we have proportional control over the elevation saturated by the tanh function. The gain of 5 tells us that the reaction becomes significant when the error in elevation is greater than 15 rad= 11 deg. For a strong enough bank ϕ, the drone loses altitude, which explains the term in |sin ϕ|. Heading. In our model of the drone, it is the bank ϕ that allows a change in the ¯ we may drone’s heading, just like a motorcycle. If we wish to have a heading of ψ, choose the following bank: ϕ¯ = tanh(5 · sawtooth(ψ¯ − ψ)). Indeed, we define the error in heading as ψ¯ − ψ that we filter using the sawtooth function (see Figure 3.2). We multiply this by the gain of the proportional controller, which is equal to 5. This gain of 5 tells us that the controller reacts significantly to

Model-free Control

175

correct the heading by the bank when sawtooth(ψ¯ − ψ) = 15 , i.e. when ψ¯ − ψ = 1 5 rad . 11 deg, which seems reasonable. We can then propose the controller: u3 = −0.3 · tanh (ϕ¯ − ϕ) where the tanh function is again used as the saturation function. Thus, the setpoint ϕ¯ for ϕ will always be within the interval [− π4 , π4 ]. Vector field. In order to define the behavior of the plane so that it may return to its circle, we assign to each position in space p = (px , py , pz ) a direction ψ¯ and an elevation θ¯ given by: √ 2 2 π p +p −¯ r ψ¯ = angle(p) + + tanh 1 50 2 2 θ¯ = −0.3 · tanh z¯−z 10

The resulting vector field has a limit cycle that corresponds to our circle or radius ¯ the expression of the error indicates us a precision of r¯. For the calculation of ψ, : 1 50 m. The component tanh 50 ( p21 + p22 − r¯) makes our circle attractive and the ¯ we have angle component (p) + π2 creates the rotating field. For the expression of θ, a saturation function that ensures a precision of 10 m in altitude and a maximum elevation of 0.2 · π2 . 0.31 rad. Summary. The controller for our drone will have the following inputs: the setpoints z¯, r¯, v¯, the Euler angles of the plane, its position p and its speed vector. It will generate the control vector u for us as given by the algorithm:

1 2 3 4

Controller in:p, v, ϕ, θ, ψ, z¯, r¯, v¯; out:u √ 2 2 p +p −¯ r π ¯ ψ = angle(p) + 2 + tanh 1 50 2 −z θ¯ = −0.3 · tanh z¯10 ϕ¯ =  tanh(5 · sawtooth(ψ¯ − ψ))  5/(1 + /tanh v −00 -v-)) / (¯ 0 u =  −0.3 · tanh 5 θ¯ − θ + |sin ϕ|  −0.3 · tanh (ϕ¯ − ϕ)

Figure 3.36 shows the result of the simulation. C ORRECTION FOR E XERCISE 3.7.– (Quadrotor)

1) We take the inverse of block (a), i.e. E   −1  d  F F τ0 ω1 β β β β F  ω2   −β* 0 β* 0   τ d1  F   = aF     ω3  G 0 −β* 0 β*   τ d2  ω4 −δ δ −δ δ τ d3

176

Mobile Robotics

√ a where v is the componentwise square-root algebraic function (for instance, : a (−4, 16, −9) = (−2, 4, −3)). Using the feed-forward block, we get that τ 0 is almost equal to τ d0 . We say “almost” due to the fact that there exists some uncertainties in the matrix that have not been taken into account.

Figure 3.36. a) Initial phase: the robot stands up and turns to its left; b) the robot returns to its circle; c) overhead view: we notice that there is a bias between the setpoint circle and the circle performed

2) We take / 0 τ d1:3 = I · kω . ω dr − ω r + ω r ∧ (I · ω r ) ,

where kω is a large gain (for instance, kω = 100) and ω dr is the desired value for ω r . Therefore, / 0 ω˙ r = I−1 · τ d1:3 − ω r ∧ (I · ω r ) /

0



  = I−1 · I · kω . ω dr − ω r + ω r ∧ (I · ω r ) − ω r ∧ (I · ω r ) 2 34 5 τd 1:3 / d 0 = kω . ω r − ω r

If the gain kω is large enough, and if ω dr varies slowly, then the error ω dr − ω r quickly converges to zero (in a time of approximately k1ω sec). 3) We take −1  d    1 tan θ sin ϕ tan θ cos ϕ ϕ ϕ − sin ϕ   θd  −  θ  ω dr = kϕθψ ·  0 cos ϕ sin ϕ cos ϕ ψ ψd 0 cos θ cos θ 

Model-free Control

Figure 3.37. Sequence of back-stepping inversions to simplify the system

177

178

Mobile Robotics

Thus, we have     d    1 tan θ sin ϕ tan θ cos ϕ ϕ˙ ϕ ϕ   − sin ϕ  θ˙  =  0 cos ϕ  · ω dr = kϕθψ ·  θd  −  θ  . sin ϕ cos ϕ ψ ψd 0 ψ˙ cos θ cos θ 

The coefficient kϕθψ should be taken as large, for instance kϕθψ = 10, but not too large or otherwise ω dr will not be smooth enough. This means that the propellers 1 are strong enough to get the right Euler angles in a time equal to 10 sec. At this level, as illustrated in Figure 3.37, we have simplified the system with a sequence of backstepping inversions. 4) We may add the following loop: ϕd θd ψd τ d0

= = = =

0.5 · tanh(10 · sawtooth(angle (fvdp (x, y)) − angle(R · vr ))) −0.3 · tanh(vd − vr1 ) angle(R · vr ) 300 · tanh(z − zd) + 60 · vr3

The equation for τ d0 corresponds to a proportional controller (300 · tanh(z − zd)) with a high gain to fight against the gravity. We also added a derivative term 60 · vr3 to avoid oscillations. These coefficients have been tuned manually, to get a good control of the altitude. The equation for ψ d says that the robot should head in the same direction as the speed vector R · vr . This property is important if for instance, we put a camera at the front of a robot that has to see ahead. The angle function computes the argument of the vector, considering the two first components only. The equation for θd states that the pitch is directly connected to the speed at which we want to go. The equation for the roll ϕd says that if we want to follow the direction of the Van der Pol field fvdp (x, y), we have to impose a roll corresponding to angle between the Van der Pol field and the trajectory of the robot. C ORRECTION FOR E XERCISE 3.8.– (Isobath) For the control of the depth, we can take a proportional control of the form u1 = y 3 − y 3 . For the heading, assume that we first want to follow the isobath just below the robot. Equivalently, we want to the robot be perpendicular to ∇h(x, y), for instance y2 = ± π2 , depending on if we want the gradient to be on our right or on our left. Take, for instance, e1 = y2 + π2 as an error, which means that we want to have an angle y 2 = − π2 with ∇h, or equivalently, we want ∇h on the right. If e1 = 0, we follow an isobath, but it may not be the right one. We thus have to consider another

Model-free Control

179

error corresponding to e2 = −y3 − y1 − h0 = 0. If e2 = 0, we are just above the right isobath but maybe not parallel to it. If both e1 = 0 and e2 = 0, we are on the right isobath (e1 = 0) and we are also going in the right direction (e2 = 0). For the heading, we may take the following controller: u2 = tanh(e2 ) + sawtooth(e1 ) = −tanh(h0 + y3 + y1 ) + sawtooth(y2 +

π ), 2

where sawtooth is needed because e1 is an error on angles, and tanh creates a saturation. The controller may thus be given by " # y3 − y 3 u= . -tanh(h0 + y3 + y1 ) + sawtooth(y2 + π2 ) The coefficients for the controller (all taken here equal to ±1) should be tuned in order to have stability and the correct time constants. N OTE .– For the heading, the controller is close to a proportional and derivative control, where tanh(h0 + y3 + y1 ) corresponds to the proportional term, and sawtooth(y2 + π2 ) to the derivative term. For the heading control, we could take a proportional and derivative control of the form " # cos ψ u2 = (y 1 − y1 ) + y˙ 1 = (y 1 − y1 ) + z˙ − ∇h(x, y) · , sin ψ

# cos ψ is sin ψ assimilated with sawtooth(y2 + π2 ). Recall that x, y, ψ are not measured and cannot be used by our controller. where y 1 = −y 3 − h0 , z˙ can be assumed to be 0 and ∇h(x, y).

"

C ORRECTION FOR E XERCISE 3.9.– (Satellite) 1) Newton’s universal law of gravitation states that the earth exerts a gravitational force f of attraction on a satellite. The direction of f is along the unit vector u directed by the line joining the two objects. The magnitude of the force is proportional to the product of the gravitational masses of the objects, and inversely proportional to the square of the distance between them. We have f = −G

Mm u, x21 + x22

where M is the mass of the earth, m is the mass of the satellite and G = 6.67x10−11 N m2 kg −2 is the Newton’s constant. We have " # Mm x1 f = −G : . 3 x2 2 2 x1 + x2

180

Mobile Robotics

Now, from Newton’s second law, we have " # dv x ¨1 f =m =m , x ¨2 dt where v is the velocity of the satellite. Thus, " # " # M x ¨1 x1 = −G : . 3 x ¨2 x2 x21 + x22

If we set α = GM , and if we take into account the possibility of controlling only the tangent acceleration, we get the following state equations:  x˙ 1 = x3     x ˙ = x4  2   αx1 x˙ 3 = − : 3 + x3 u  x21 + x22   αx2    3 + x4 u  x˙ 4 = − : 2 x1 + x22 2) To apply a feedback linearization method, take as an output y = x21 + x22 − R2 . We have: y = x21 + x22 − R2 y˙ = 2x1 x3 + 2x2 x4 y¨ = 2x˙ 1 x3 + 2x>1 x˙ 3 + 2x˙ 2 x4 + 2x2 x˙ 4? > ? x1 x2 2 2 = 2x3 + 2x1 − : 3 + x3 u + 2x4 + 2x2 − : 3 + x4 u x21 + x22 x21 + x22 / 0 2 = 2 x23 + x24 − : 2 + (2x1 x3 + 2x2 x4 )u 34 5 x1 + x22 2 a(x)

[3.10]

A feedback linearization generates a singularity for a(x) = 2x1 x3 + 2x2 x4 = 0, which corresponds to y˙ = 0. Now, if we want to have y → 0, we will also have y˙ → 0 and we will thus converge to the singular surface a(x) = 0, which is not acceptable. 3) For y(t) ≡ 0, we have y = y˙ = y¨ = 0, i.e. x21 + x22 − R2 = 0 2x1 x3 + 2x2 x4 = 0 / 2 0 2 2 2 x3 + x4 − : 2 + (2x1 x3 + 2x2 x4 ) u = 0 x1 + x22

Model-free Control

181

or equivalently, e1 (x) = x21 + x22 − R2 = 0 e2 (x) = x1 x3 + x2 x4 = 0 e3 (x) = x23 + x24 − R1 = 0 The error e1 corresponds to the error with respect to the potential energy, e2 is the derivative of e1 and e3 is the error with respect to the kinetic energy. The controller we choose is the following: u = −e1 (x) − e2 (x) − e3 (x) . The sign for the coefficients of the controller are chosen has follows: when e1 > 0, e2 = 0, e3 = 0 the satellite has too much energy and should slow down, but not too fast (this is why we need a derivative term corresponding to e2 ). When e1 = 0, e2 = 0, e3 > 0, the satellite has too much kinetic energy and should also slow down. Figure 3.38 shows the behavior of our controller. The blue disk corresponds to the planet, the red disk corresponds to the initial position and the black circle is the circle to follow: x21 + x22 − R2 = 0. On the left, we have no control (u = 0) and the satellite follows an ellipsoid. In the figure in the center, we took u = −e1 (x) − e3 (x) without any derivative term. The trajectory oscillates in the neighborhood of the desired circle. The right figure corresponds to the controller we propose. The trajectory converges to the desired circle. Note that the method we proposed here is based on the physical intuition that tells us to stabilize both the potential and kinetic energies. Without this intuition, it would have been difficult to solve our stabilization problem.

Figure 3.38. Left: u = 0. Center: u = −e1 (x) − e3 (x). Right: u = −e1 (x) − e2 (x) − e3 (x). For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

4 Guidance

In previous chapters, we have studied how to build a control law for a robot described by nonlinear state equations (see Chapter 2) or when the robot’s behavior is known (see Chapter 3). Guidance is performed on a higher level and focuses on the setpoint given to the controller in order for the robot to be able to accomplish its assigned mission. It will, therefore, have to take into account the knowledge of its surroundings, the presence of obstacles, the roundness of the Earth, and so forth. Conventionally, guidance is applied in four different environments: terrestrial, marine, aerial and spatial. Given the fields of application covered in this book, we will not study the spatial environment. 4.1. Guidance on a sphere For longer paths over the surface of the Earth, the Cartesian coordinate system – which assumes a flat Earth – can no longer be considered. We then have to rethink our control laws by navigating relative to a spherical coordinate system (also referred to as geographical coordinates), which rotates together with the Earth. Let us denote by *x the longitude and by *y the latitude of the point being considered. The transformation in the geographical coordinate system is written as:       *x x ρ cos *y cos *x T :  *y  →  y  =  ρ cos *y sin *x  [4.1] ρ z ρ sin *y When ρ = 6, 370 km, we are on the surface of the Earth, which we will assume to be spherical (see Figure 4.1(a)). Let us consider two points a and m on the surface of the Earth, as illustrated on Figure 4.1(b), located by their geographical coordinates. Here, a for instance Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

184

Mobile Robotics

represents a reference point to reach and m is the center of our robot. By assuming that the two points a and m are not too far apart (by no more than 100 km), we may consider being in the plane and use a local map, as shown on Figure 4.2(a).

Figure 4.1. a) Geographical coordinate system; b) we would like to express a in the local map R1

Figure 4.2. a) The map gives a local Cartesian viewpoint around the robot; b) a slight shift d'x , d'y , dρ creates a displacement dx, dy, dz in the local map

Guidance

185

Let us differentiate relation [4.1]. We obtain: 

     dx −ρ cos *y sin *x −ρ sin *y cos *x cos *y cos *x d*x  dy  =  ρ cos *y cos *x −ρ sin *y sin *x cos *y sin *x  ·  d*y  dz 0 ρ cos *y sin *y dρ 2 34 5 =J

This formula can be used to find the geographical coordinates of the cardinal directions, which change depending on the location of the robot m. For instance, the vector corresponding to East is found in the first column of the matrix J, North in the second column and the altitude in the third column. We will, therefore, be able to build a coordinate system (East-North-Altitude) R1 centered on the robot (in gray in Figure 4.2(a)) that corresponds to the local map. The corresponding rotation matrix is obtained by normalizing each column of the Jacobian matrix J. We obtain:   − sin *x − sin *y cos *x cos *y cos *x − sin *y sin *x cos *y sin *x  R =  cos *x [4.2] 0 cos *y sin *y This matrix can also be expressed in terms of Euler matrix as: π π. R = Reuler (0, 0, *x ) · RTeuler 0, *y − , − . 2 2

The transformation that allows switching from the geographic coordinate system R0 to the local map R1 is: v|R1 = RT · v|R0

[4.3]

This coordinate system change relation can be applied in different contexts such as for instance when establishing coherence in the data collected by two different robots. / 0 m E XAMPLE.– A robot situated at m: *m is moving with a speed vector vm , x , *y relative to a fixed ground. This vector is expressed in the local map of the robot. We want to find the speed /with which this vector vm is perceived in the local map of an 0 a a observer situated at a: *x , *y . Following [4.3], we have: A

/ 0 m v m = RT * m · v|R0 x , *y / 0 va = RT *ax , *ay · v|R0

Therefore:

/ 0 / 0 m m va = RT *ax , *ay · R *m ·v . x , *y

186

Mobile Robotics

This kind of calculation is useful when, for instance, two robots are trying to meet. Switching to a local map. Let us take a local map Rm centered on a point m, (i.e. a coordinate system located on the surface of the Earth whose origin is m and whose directions are East-North-upwards. Let us now try to express in Rm the coordinates (@ x, y@, z@) of a point a located by its GPS coordinates (*x , *y , ρ). We can switch from the geographical coordinates to the local coordinates with the following relation:       − sin *m cos *m 0 x @ 0 x x m m  y@  +  0  [4.3]  − sin *m cos *m =  − cos *m x sin *y x sin *y y m m m m z@ ρm cos *m cos * cos * sin * sin * x y y x y 2 34 5 2 34 5 oa|Rm m RT ()m ,) ) x y   ρ cos *y cos *x · ρ cos *y sin *x  ρ sin *y 2 34 5 oa|R0

where o is the origin corresponding to the center of the Earth. When *m x . *x and *m . * , a first-order approximation is directly obtained with the aid of Figure 4.3. y y     ρ · cos /*y · (*x −0*m x @ x )  y@  .  . ρ · *y − *m [4.4] y m z@ ρ−ρ

Figure 4.3. Getting Cartesian coordinates in a local frame centred in m

Note that the circle of latitude has a radius of ρ cos *y .

Guidance

187

When the robot is moving in a small area, we often choose a reference point corresponding to its launch position. 4.2. Path planning When the robot is completely autonomous, the desired path must be planned out [LAV 06]. Very often, these paths are polynomials, for two reasons. First of all, the space of polynomials has a vector space structure and can therefore utilize the power of linear algebra. Second, they are easier to differentiate, which is useful for feedback linearization, since it requires the successive derivatives of the setpoints. 4.2.1. Simple example Let us illustrate how such a planning is performed on the example of a robot tank. Assume that at the initial moment t = 0, the robot is located at the point (x0 , y0 ) and that we would like to reach the point (x1 , y1 ) at time t1 with a speed equal to (vx1 , vy1 ). We suggest a polynomial path of the form: xd = ax t2 + bx t + cx yd = ay t2 + by t + cy We need to solve the system of equations: c x = x0 , cy = y 0 ax t21 + bx t1 + cx = x1 ay t21 + by t1 + cy = y1, 2ax t1 + bx = vx1 , 2ay t1 + by = vy1 which is linear. We easily obtain:    1 x − 1 x + 1 v1  ax t1 x t21 0 t21 1 1 1 1 1   ay   y − y + 2 2  0 1 t1 v y  t    t1  bx   −v 1 − 21 x + 2 x  0 1   = x  by   −v 1 − t21 y + t21 y      y t1 0 t1 1   cx    x0 cy y0 We therefore have:

x˙ d = 2ax t + bx y˙ d = 2ay t + by x ¨d = 2ax , y¨d = 2ay By inserting these quantities into a control law obtained using linearizing feedback (as is the case for instance with equation [2.9]), we obtain a controller that meets our objectives.

188

Mobile Robotics

4.2.2. Bézier polynomials Here we will look at generalizing the approach presented in the previous section. Given the control points p0 , p1 , . . . , pn , we can generate a polynomial f (t) so that f (0) = p0 , f (1) = pn and so that for t ∈ [0, 1], the polynomial f (t) is successively attracted by the pi with i ∈ {0, . . . , n}. In order to correctly understand the method of building Bézier polynomials, let us examine various cases: – case n = 1. We take the standard linear interpolation: f (t) = (1 − t) p0 + tp1 . The point f (t) corresponds to a barycenter between the control points p0 and p1 , and the weights assigned to these two points change with time: – case n = 2. We now have three control points p0 , p1 , p2 . We create an auxiliary control point p01 which moves on the segment [p0 , p1 ] and another p12 which is associated with the segment [p1 , p2 ]. We take: f (t) = (1 − t) p01 + tp12 = (1 − t) ((1 − t) p0 + tp1 ) + t((1 − t) p1 + tp2 ) 2 34 5 2 34 5 p01

2

p12

= (1 − t) p0 + 2 (1 − t) tp1 + t2 p2 .

We thus obtain a second-order polynomial; – case n = 3. We apply the previous method for four control points. We obtain: f (t) = (1 − t) p012 + tp123 = (1 − t) ((1 − t) p01 + tp12 ) + t((1 − t) p12 + tp23 ) 2 34 5 2 34 5 p012

3

2

p123

= (1 − t) p0 + 3 (1 − t) tp1 + 3 (1 − t) t2 p2 + t3 p3

– for a given n, we obtain: f (t) =

n 1

n! n−i i (1 − t) t pi . i! (n − i)! i=0 2 34 5 bi,n (t)

The polynomials bi,n (t), called Bernstein polynomials, form a basis of the space of n-degree polynomials. When we increase the degree (i.e. the number of control points), numerical instability and oscillations appear. This is called Runge’s phenomenon. For complex curves with hundreds of control points, it is preferable to

Guidance

189

use B-splines corresponding to a concatenation of Bézier curves of limited order. Figure 4.4 illustrates such a concatenation in which, for each group of three points, we can calculate a second-order Bézier polynomial.

Figure 4.4. Illustration of second-order B-splines

4.3. Voronoi diagram Let us consider n points p1 , . . . , pn . Contrary to the previous sections, the pi here do not correspond to control points, but to point obstacles that we try to avoid. To each of these points, we associate the set: + , Pi = x ∈ Rd , ∀j, -x − pi - ≤ -x − pj - .

For all i, this set is a polygon. The collection of these Pi is called a Voronoi diagram. Figure 4.5 represents a set of points with the associated Voronoi diagram. If an environment contains obstacles, the robot will have to plan a path that remains on the borders of the Pi . Delaunay triangulation. Given n points in space, we can use the Voronoi diagrams to perform a triangulation of the space. This corresponding triangulation, referred to as Delaunay triangulation, allows maximizing the quantity of acute angles and thus avoid elongated triangles. It is obtained by connecting the neighboring points of the corresponding regions with an edge in the Voronoi diagram. In a Delaunay triangulation, none of the triangles contains another point within its circumscribed circle. Figure 4.6 represents the Delaunay triangulation, associated with the Voronoi diagram in Figure 4.5. A Delaunay triangulation is often used in robotics to represent space such as for example the area already explored, restricted areas, lakes, etc. We

190

Mobile Robotics

often associate a color with each triangle following the characteristics of the space the triangle belongs to (water, land, road, etc.).

Figure 4.5. Voronoi diagram

Figure 4.6. Delaunay triangulation

Guidance

191

4.4. Artificial potential field method A mobile robot has to move in a congested environment that contains mobile and stationary obstacles. The artificial potential field method [LAT 91] consists of imagining that the robot can behave like an electric particle that can be attracted or repelled by other objects following the sign of their electric charge. This is a reactive approach to guidance in which the path is not planned in advance. In physics, we have the following relation: f = −gradV (p) , where p is the position of point particle in space, V is the potential and f the force applied on the particle. We will have the same relation in mobile robotics, but with p the position of the center of the robot, V a potential imagined by the robot and f the speed vector to follow. The potential fields will help us express a desired behavior for a robot. The obstacles will be represented by potentials exerting a repulsive force on the robot while the objective to follow will exert an attractive force. In a situation where several robots need to remain grouped while avoiding collisions, we can use a near-field repulsive potential and a far-field attractive potential. More generally, the vector fields used might not derive from a potential, as is the case if we would like the robot to have a cyclic behavior. The following table gives several types of potential that can be used: Potential attractive conical

V (p) -p − p ˆ-

−grad(V (p)) p−ˆ p − &p−ˆ p&

−ˆ vT · p

v ˆ

2

attractive quadratic -p − p ˆ−2 (p − p ˆ) T attractive plane or line (p − p ˆ) · n ˆn ˆ T · (p − p ˆ ) −2 n ˆn ˆ T (p − p ˆ) (p−ˆ q) 1 repulsive &p−ˆ q& &p−ˆ q &3 uniform

In this table, p ˆ represents an attractive point, q ˆ a repulsive point and v ˆ a desired speed for the robot. In the case of the attractive plane, p ˆ is a point of the plane and n ˆ is a vector orthonormal to the plane. By adding several potentials, we can ask the robot (which is supposed to follow the direction that tends to decrease the potential) to accomplish its objectives while moving away from the obstacles. Figure 4.7 represents three vector fields derived from artificial potentials. The one on the left corresponds to a uniform field, the one in the middle to a repulsive potential that is added to a uniform field and the one on the right to the sum of an attractive potential and a repulsive potential.

192

Mobile Robotics

Figure 4.7. Artificial potential fields

4.5. Exercises E XERCISE 4.1.– Pursuit on a sphere Consider a robot R moving on the surface of a sphere similar to that of the Earth, with a radius of ρ = 30 m. This robot is located by its longitude *x , its latitude *y and its heading ψ, relative to the East. In a local coordinate system, the state equations of the robot are of the type:   x˙ = cos ψ y˙ = sin ψ  ˙ ψ = u

1) Give the state equations in the case in which the state vector is (*x , *y , ψ). 2) Simulate this evolving system graphically in 3D. 3) A second robot Ra , described by the same equations, is moving randomly on the sphere (see Figure 4.8). Suggest a control law that allows the robot R to meet the robot Ra . E XERCISE 4.2.– Planning a path Let us consider a scene with two triangles as shown on Figure 4.9, and a robot described by the state equations:  x˙ = v cos θ    y˙ = v sin θ  θ˙ = u1   v˙ = u2

Guidance

193

with initial state (x, y, θ, v) = (0, 0, 0, 1). This robot has to reach the point with coordinates (8, 8).

Figure 4.8. On the sphere, the robot R follows the robot Ra

Figure 4.9. The robot has to follow a path without hitting the obstacles

194

Mobile Robotics

1) Write a program in order to find the control points for a Bézier polynomial that connects the initial position to the desired position as shown on the Figure. 2) Using feedback linearization, deduce the control law that allows to reach the objective in 50 sec. E XERCISE 4.3.– Drawing a Voronoi diagram Let us consider the ten points in Figure 4.10. Draw the associated Voronoi diagram on a piece of paper as well as the corresponding Delaunay triangulation.

Figure 4.10. Ten points for which we want to build a Voronoi diagram

E XERCISE 4.4.– Calculating a Voronoi diagram 1) Show that if x and y are two vectors of Rn , we have the so-called polarization equations: +

2

2

2

-x − y- = -x- + -y- − 2 *x, y+

For this, develop the expression of the scalar product *x − y, x − y+.

2) Let us consider n + 1 points a1 , . . ., an+1 and their circumscribed sphere denoted by S. With the aid of the previous question, give an expression, in function of the ai , of the center c of S and of its radius r. 3) Let us now consider three points in the plane a1 , a2 , a3 . Which conditions / 0must be verified for m to be within the circle circumscribed to the triangle a1 , a2 , a3 ?

Guidance

195

4) Consider m points in the plane p1 , ./. . , pm , a Delaunay triangulation is a 0 partition of the space into triangles T (k) = a1 (k) , a2 (k) , a3 (k) , whose vertices are taken from the pi such that each circle C (k) circumscribed to this triangle T (k) contains none of the pi . Write a program that takes m = 10 random points of the plane and draws a Delaunay triangulation. What is the complexity of the algorithm? 5) Given the triangulation established in the previous question, build a Voronoi diagram associated with the points a1 , . . ., an+1 . E XERCISE 4.5.– Heading control of a Dubins car The results of this exercise will be used in exercise 4.6 for calculating Dubins paths. A Dubins car is described by the state equations:   x˙ = cos θ y˙ = sin θ ˙ θ = u

θ is the robot’s heading and (x, y) the coordinates of its center. This robot has to be ¯ aligned with a heading setpoint θ. 1) We assume that the input u ∈ [−1, 1]. Give an analytic expression of the error angle δ ∈ [−π, π] in function of θ and θ¯ that indicates the angle the robot has to turn by in order to reach its setpoint as fast as possible. Taken into account that the ¯ Indeed, the angles of −π, π or expression of δ has to be periodic relative to θ and θ. 3π have to be considered as equivalent. Give the associated control law and simulate it. 2) Same as above, with the exception that the robot can only turn left (in the direct trigonometric sense), i.e. u ∈ [0, 1]. Let us note that now, δ ∈ [0, 2π]. 3) Same as above, but the robot can now only turn right, i.e. u ∈ [−1, 0].

E XERCISE 4.6.– Dubins paths by:

As in the previous exercise, let us consider a robot moving on a plane, described   x˙ = cos θ y˙ = sin θ ˙ θ = u

where θ is the robot’s heading and (x, y) the coordinates of its center. Its state vector is given by x = (x, y, θ) and its input u must remain within the interval [−umax , umax ]. This is the Dubins car [DUB 57] corresponding to the simplest possible nonholonomic

196

Mobile Robotics

mobile vehicle. Despite its simplicity, it illustrates many difficulties that may appear within the context of nonholonomic robots. 1) Calculate the maximum radius of curvature r that can be executed by the path of the robot. 2) Dubins showed that in order to switch from a configuration a = (xa , ya , θa ) to a configuration b = (xb , yb , θb ) that are not too close together (i.e. separated by a distance superior to 4r), the minimum time strategy always consists of (1) turning to the maximum in one direction (i.e. u = ±umax ); (2) moving straight ahead; (3) then turning to the maximum again. The path corresponding to such a maneuver is called a Dubins path, and is thus composed of a starting arc, a segment and a termination arc. There are four ways to construct a Dubins path: LSL, LSR, RSL, RSR, where L means left, R means right and S stands for straight ahead. Give a configuration for a and b such that none of the four paths corresponds to an optimal strategy (and therefore a and b are quite close together). A situation in which the optimal strategy is RLR will be chosen. 3) In the case of an RSL strategy as illustrated on Figure 4.11, calculate the length L of the Dubins path in function of a and b.

Figure 4.11. In bold: an RSL-type ( Right-Straight-Left) Dubins path leading from a to b; in dotted: an RLR-type Dubins path

4) In the case of an LSL strategy, calculate the length L of the Dubins path in function of a and b. 5) By using the reflection symmetry of the problem, deduce L in the case of RSR and LSR strategies, then write a function that calculates L in all situations. For this, the two Booleans εa , εb will be used, which are equal to 1 if the corresponding arc is in the forward direction (i.e. to the left) and −1 otherwise.

6) Use the previous questions to write program that calculates the minimum length path for a Dubins car.

Guidance

197

E XERCISE 4.7.– Artificial potentials A robot situated at p = (x, y) must reach a target of unknown movement whose position p ˆ and speed v ˆ are known at the present time. This pair (ˆ p, v ˆ) might for instance correspond to a setpoint given by a human operator. A fixed obstacle located at position q ˆ must be avoided. We model the desired behavior of our robot by the potential: V (p) = −ˆ vT · p + -p − p ˆ- + 2

1 -p − q ˆ2

where the potential −ˆ vT · p represents the speed setpoint, the potential -p − p ˆ1 makes the target position p ˆ attractive and the potential &p−ˆ makes the obstacle q ˆ q& repulsive. 1) Calculate the gradient of the potential V (p) and deduce the speed vector setpoint w (p, t) to apply to our robot so that it responds correctly to this potential. 2) We assume that our robot obeys the following state equations:  x˙    y˙ v˙   ˙ θ

= = = =

v cos θ v sin θ u1 u2

Give the control law that corresponds to the desired potential field. We will use the same principle as the one shown in Figure 4.12. First of all, disassemble the robot (in grey on the figure) into a chain made of two blocks. The first block forms the speeds from the actuators and the second builds the position vector p = (x, y). Then calculate the left inverse of the first block in order to end up with a system of the type: !

x˙ = v¯ cos θ¯ y˙ = v¯ sin θ¯

Use a simple proportional / 0 control to perform this approximate inversion. Then, generate the new input v¯, θ¯ using the potential to be satisfied. Illustrate the behavior of the robot with a target p ˆ = (t, t) and a fixed obstacle placed at q ˆ = (4, 5). 3) We would now like to follow the target p ˆ with a mobile obstacle at q ˆ with: p ˆ=

"

t cos 10 t 2 sin 10

#

and q ˆ=

"

2 cos 5t 2 sin 5t

#

.

Adjust the parameters of the potential in order to follow the target without hitting the obstacle. Illustrate the behavior of the controlled robot.

198

Mobile Robotics

Figure 4.12. Controller (dotted) obtained by the potential method

E XERCISE 4.8.– Flocking We consider m = 20 robots, described by the following state equations:   x˙ i = cos θi y˙ i = sin θi ˙ θi = u i

The state vector is x(i) = (xi , yi , θi ). These robots can see all other robots, but are not able to communicate with them. We want these robots to behave as a flock, as illustrated in Figure 4.13. The basic models of flocking behavior are controlled by Reynolds’ three rules: separation (short-range repulsion), alignment and cohesion (long-range attraction). Using a potential-based method, find a controller for each robot to obtain a flock. E XERCISE 4.9.– Platooning We consider m = 10 robots turning on a circular road of circumference L = 100 m L and with radius r = 2π . Each robot Ri satisfies the following state equations: !

a˙ i = vi v˙ i = ui

The state vector is x(i) = (ai , vi ), where ai corresponds to the position of the robot and vi to its speed. Each robot Ri is equipped with a radar which returns the distance di to the previous robot Ri−1 and its derivative d˙i , as illustrated in Figure 4.14.

Guidance

Figure 4.13. Illustration of the flocking behavior from a random initialization

Figure 4.14. Platooning on the circle

199

200

Mobile Robotics

1) Write the expression - of the . observation function g (ai− , ai , vi− , vi ), which ˙ returns the vector y(i) = di , di . In this formula, i− = i − 1 if i > 0 and i− = m if i = 0. Indeed, since the road is circular, the robot R1 follows the robot Rm .

2) Propose a proportional and derivative control so that the robots will have a uniform distribution and move at a speed equal to v0 = 10 ms−1 . Check it with a simulation. 3) In the case of stability, prove theoretically that when the steady behavior is reached, all of the robots have a speed equal to v0 and are uniformly distributed. 4) Prove the stability of the system for four robots. 5) Provide a simulation with 10 robots. E XERCISE 4.10.– Action on a vector field

Given a state equation x˙ = f (x) where x ∈ Rn and a diffeomorphism g : Rn → Rn . The action by g on f is defined by g•f =

"

dg ◦ g−1 dx

#

/ 0 · f ◦ g−1 .

1) Show that if y = g(x), then we have y˙ = g • f (y). As a consequence, the action by g on the system x˙ = f (x) is the new system y˙ = (g • f ) (y). 2) Consider the system in polar coordinates as / 0# " # " r˙ −r r2 − 1 = , 1 θ˙ where r ≥ 0. Study the behavior and the stability of the system. 3) Define the vector x = g(r, θ) = r

"

cos θ sin θ

#

where g is seen as an action. Show that x˙ =

"

−x31 − x1 x22 + x1 − x2 −x32 − x21 x2 + x1 + x2

#

and draw the vector corresponding vector field.

Guidance

201

4) Take g(x) = Ax, and show that (g • f ) (x) = A · f (A−1 · x) 5) From the previous questions find the expression of a vector field such that the limit cycle is clockwise stable and corresponds to an ellipse with a major radius 2, a minor radius 1 and an inclination of π4 . Draw the corresponding vector field. E XERCISE 4.11.– Eight circuit Consider a robot moving on a plane and described by the following state equations:   x˙ 1 = cos x3 x˙ 2 = sin x3  x˙ 3 = u

where x3 is the heading of the robot and (x1 , x2 ) are the coordinates of its center. The state vector is given by x = (x1 , x2 , x3 ). In this exercise, we want to find a controller so that the robot follows a path with a eight shape. 1) The expression of a vector field converging counterclockwise to a circle of radius 1 is given by "

p˙ 1 p˙ 2

#

= Φ0 (p) =

"

−p31 − p1 p22 + p1 − p2 −p32 − p21 p2 + p1 + p2

#

Find the expression of a vector field Φε,ρ,c attracted by a circle of radius ρ and center c, where the attraction is counterclockwise of ε = 1 and clockwise if ε = −1.

2) Propose a controller so that the robot follows counterclockwise a circle with a radius ρ = 2 and a center c = (2, 0). Illustrate the behavior of the controller by simulation. The wanted behavior of the controller is illustrated by Figure 4.15. 3) Considering two vector fields of the form Φε,ρ,c , build an automaton with four states so that the robot is attracted by a cycle corresponding to an eight circuit as illustrated by Figure 4.16. 4.6. Corrections C ORRECTION FOR E XERCISE 4.1.– (Pursuit on a sphere) 1) Following [4.4], we have the following first-order approximation "

dx dy

#

.

"

ρ cos *y d*x ρd*y

#

.

202

Mobile Robotics

Figure 4.15. The car is attracted counterclockwise by the circle

Figure 4.16. The robot following an eight circuit

Guidance

203

Therefore, since x˙ = cos ψ and y˙ = sin ψ, we have  cos ψ ˙    *x = ρ cos *y sin ψ *˙y = ρ   ˙ θ = u.

2) In order to draw the robot we first create a 3D pattern, then subject it to a coordinate system change which will lead it to the desired area (see equation [4.2]) and finally apply an Euler transformation in order to give it the correct local orientation. Note that in our case, the elevation θ and the bank ϕ are zero, since the robot is moving horizontally. If the robot R located at the pose (*x , *y , ψ), we obtain the following transformation matrix:     − sin *x − sin *y cos *x cos *y cos *x cos *y cos *x  cos *x − sin *y sin *x cos *y sin *x  · R (0, 0, ψ) ρ  cos *y sin *x      0 cos *y sin *y sin *y     / 0 0 0 0 1

3) The robot R located at m : (*x , *y ) with of ψ relative to the East / a heading 0 must reach robot Ra located at the point a : *ax , *ay . If a is in a close neighborhood of m (less than 100 km), we can calculate the coordinates of a within a map centered around m. We obtain, following formula [4.4] ! a a a x @ = ρ cos / a *y · (*0x − *x ) a y@ = ρ *y − *y . In order to find out whether the robot Ra is on the left or the right of R, we look at the sine of the angle between the two vectors (cos ψ, sin ψ) and (@ xa , y@a ). We will, therefore take the proportional controller: " # cos ψ cos *y · (*ax − *x ) u = det . sin ψ *ay − *y

As illustrated by Figure 4.17, this control law, which is based on a local representation of the target Ra position works correctly, even when Ra is far away from R. C ORRECTION FOR E XERCISE 4.2.– (Path planning) 1) Given the control points, the associated setpoint function is given by: w (t) =

n 1

n! n−i i (1 − t) t pi . i! (n − i)! i=0 2 34 5 bi,n (t)

204

Mobile Robotics

Figure 4.17. The blue robot R follows the red one Ra . For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

We can easily adjust the control points in order to obtain a desired path. For example, we will take the control points pi given in the matrix: " # 1 1 1 1 2 3 4 5 5 7 8 10 9 8 1 4 7 9 10 8 6 4 1 0 0 1 4 8 and which are represented on the left of Figure 4.18. 2) We can take the controller found in section 2.4.1. To reach the objective in tmax = 50 sec, we must take the setpoint w (t) =

n 1

t bi,n ( 50 ) pi .

i=0

In order to apply our controller, we need the derivative w ˙ (t) of the setpoint w (t). It is given by w ˙ (t) =

1 50

n 1 i=0

t b˙ i,n ( 50 ) pi

Guidance

with b˙ i,n (t) =

205

. n! n−i i−1 n−i−1 i i (1 − t) t − (n − i) (1 − t) t . i! (n − i)!

We must make sure to take into account the particular case of i = 0 for which n−1 b˙ i,n (t) = −n (1 − t) and the case of i = n for which b˙ n,n (t) = ntn−1 . The resulting path is represented on the right of Figure 4.18.

Figure 4.18. Left: control points with the associated Bézier polynomial; right: path for the robot that follows the polynomial by avoiding the obstacles

C ORRECTION FOR E XERCISE 4.3.– (Drawing a Voronoi diagram) On Figure 4.19, the Delaunay triangulation is represented by the bold lines. The Voronoi diagram is represented by the thin lines. Figure 4.20 shows that the circle circumscribed to each triangle does not enclose any other point. C ORRECTION FOR E XERCISE 4.4.– (Calculating a Voronoi diagram) 1) Recall that the scalar product is a bilinear form. We therefore have: 2

-x − y- = *x − y, x − y+ = *x, x+ + *y, y+ − *x, y+ − *y, x+ 2

2

= -x- + -y- − 2 *x, y+ . 2) Let c be the center of the sphere S. For i ∈ {1, . . . , n + 1}, we have: H i H Ha − c H2 = r 2

206

Mobile Robotics

Figure 4.19. Voronoi diagram

Figure 4.20. Circles circumscribed to the Delaunay triangles

And, by using the polarization equation H i H2 I J Ha H + -c-2 − 2 ai , c − r2 = 0.

Guidance

207

Of course, we have n + 1 unknowns which are r and c. In order to obtain a linear system in these unknowns, let us take: 2

cn+1 = -c- − r2 . Thus: H H2 I J 2 ai , c − cn+1 = Hai H .

We therefore have: 

2a11 · · · 2a1n  .. ..  . . 2an+1 · · · 2an+1 n 1

   H 1 H2   c Ha H −1  .1    .    .. .. . = . .   c n   H . H2  Han+1 H −1 cn+1

Solving the linear system yields < the center of the circumscribed circle 2 c = (c1 , . . . , cn ) as well as its radius r = -c- − cn+1 . / 0 3) The point m is within the circle circumscribed to the triangle a1 , a2 , a3 if: with:

< 2 -m − c- ≤ -c- − ρ

H    1 −1  H Ha1 H 2 2a1 2a12 −1 c1 H H2   c2  =  2a21 2a22 −1    Ha2 H  . H 3 H2 ρ 2a31 2a32 −1 Ha H 

4) The following program compute the matrix K containing the triangles corresponding to the Delaunay triangulation. Function D ELAUNAY(P) 1 m := size(P) ; K := ∅ 2 For (i, j, with 1 ≤0i ≤ j ≤ k ≤ m / k) a1 , a2 , a3 := (P(:, i), P(:, j), 3 P(:, k)) H2   1 −1  H 1 H " # a1 H 2a1 2a2 −1 c  H H2  4 :=  2a21 2a22 −1   Ha2 H  ρ H 3 H2 2a31 2a32 −1 Ha H    " # < i K 2 K ,  j  5 if q∈{i,j,k} -P(:, q) − c> -c− ρ then K := / k 6 Return K

208

Mobile Robotics

The complexity of the algorithm is in m4 , where m is the number of points being considered. There are algorithms in m log m such as the sweep line algorithm and Fortune’s algorithm. 4) To draw the Voronoi diagram, we connect all the centers of the circles circumscribed to the neighboring triangles C ORRECTION FOR E XERCISE 4.5.– (Heading control of a Dubins car) 1) Let us recall the expression of the sawtooth function (see formula [3.2]): sawtooth (θ) = mod(θ + π, 2π) − π. @ Let us take θ@ = θ¯ − θ. In order to have δ ∈ [−π, π], we will take δ = sawtooth(θ). @ @ Note that if θ ∈ [−π, π] then δ = θ. In order to have u ∈ [−1, 1], we can take a proportional control law of the type: u=

δ 1 @ = sawtooth(θ). π π

2) We write the directional sawtooth function as follows: sawtooth (θ, d) = mod(θ + (1 − d)π, 2π) + (d − 1)π. This function returns the distance to the left (d = 1), to the right (d = −1) or the shortest distance (d = 0) as illustrated by Figure 4.21. The shortest distance case corresponds to the usual sawtooth function. In our case, d = 1 and therefore δ = @ 1). In order to have u ∈ [0, 1], we need to take: sawtooth(θ, u=

@ 1) δ sawtooth(θ, = . 2π 2π

@ −1) − π. To have u ∈ [−1, 0], we take: 3) Now, d = −1. We take δ = sawtooth(θ, u=

@ −1) δ sawtooth(θ, = . 2π 2π

C ORRECTION FOR E XERCISE 4.6.– (Dubins path) 1) For u constant, we have θ = ut + θ0 . And therefore x˙ (t) = cos (ut + θ0 ) and y˙ (t) = sin (ut + θ0 ). And, after integrating: x (t) = x (0) + u1 sin (ut + θ0 ) and y (t) = y (0) − u1 cos (ut + θ0 ). The associated curvature radius is r = u1 . Since 1 1 u ∈ [−umax , umax ], we have r ∈ [−umax1,umax ] = [−∞, − umax ] ∪ [ umax , ∞].

Guidance

209

Figure 4.21. Sawtooth functions for performing the angular difference and rejoining the desired interval

2) We can take b at a distance of 3.9r to the right of a. The optimal strategy is RLR. 3) As illustrated on Figure 4.23, the vectors da −ca are orthogonal. Thus, by using the Pythagorean theorem, the half-length * of the Dubins path segment is given by: *=

L"

-cb − ca 2

#2

− r2 .

If this square root is not defined (i.e. if -cb − ca - ≤ 2r), then this path is not valid and will not correspond to a Dubins path. As a matter of fact, it is in this situation that an RLR strategy could be optimal.

210

Mobile Robotics

Figure 4.22. Top: the four superimposed Dubins paths of type RSR, LSR, RSL and LSL. Bottom: the best of the four paths together with the RLR strategy (in bold) which proves to be the best among all strategies

Figure 4.23. Calculating the length of the RSL Dubins path based on the fact that da − ca ⊥ db − da ⊥ db − cb

In the case where -cb − ca - ≥ 2r, calculating the path length is done as follows: 1 2 3 4 5 6 7 8

# " # " # # " xb − sin θb xa − sin θa ; cb := +r ca := −r yb cos θb cos θa < ya 2 1 * := 4 -cb − ca - − r2 . If * is not real, return L = ∞ (fail) α := atan2 (*, r) " # cos α − sin α r da := ca + &cb −c · · (cb − ca ) a& sin α cos α db := cb + ca − da βa := sawtooth (angle ((xa , ya ) − ca , da − ca ) , 1) βb := sawtooth (angle (db − cb , (xb , yb ) − cb ) , 1) L := r |βa | + r |βb | + 2* "

Guidance

211

4) Calculating the length of the path is done as follows: # " # " # " # xa − sin θa xb − sin θb +r ; cb := +r ya cos θa yb cos θb π * := 12 -cb − ca - ; α := − 2 " # cos α − sin α r da := ca + &cb −c · · (cb − ca ) a& sin α cos α db := cb + da − ca βa := sawtooth (angle ((xa , ya ) − ca , da − ca ) , 1) βb := sawtooth (angle (db − cb , (xb , yb ) − cb ) , 1) L := r |βa | + r |βb | + 2*

1 ca := 2 3 4 5 6 7

"

5) The following function computes L in all situations: Function PATH(a, b, r, ε"a , εb# ) " # − sin θa xa 1 (xa , ya , θa ) := a; ca := + εa r " cos θa # " ya# − sin θb xb 2 (xb , yb , θb ) := b; cb := + εb r cos θb yb 3 If εa · εb = −1 < 2

4 5 6

7 8 9 10

* := 14 -cb − ca - − r2 . If * is not real, return L = ∞ α := −εa · atan2(*, r); else * := 12 -cb"− ca - ; α := −εa#π2 cos α − sin α r da := ca + &cb −c · · (cb − ca ) a& sin α cos α db := cb + εa · εb (da − ca ) βa := sawtooth (angle ((xa , ya ) − ca , da − ca ) , εa ) βb := sawtooth (angle (db − cb , (xb , yb ) − cb ) , εb ) Return L := r |βa | + r |βb | + 2*

5) We test all four possible paths (εa = ±1, εb = ±1) and take the best one. For r = 10, a = (−20, 10, −3) and b = (20, −10, 2), we obtain the results of Figure 4.24. For a = (−3, 1, 0.9) and b = (2, −1, 1) we obtain those of Figure 4.25. C ORRECTION FOR E XERCISE 4.7.– (Artificial potentials) 1) We have: T

dV (p − q ˆ) T (p) = −ˆ vT + 2 (p − p ˆ) − 3 dp -p − q ˆ-

212

Mobile Robotics

Figure 4.24. Left: the four Dubins paths possible; right: the best of the four paths

Figure 4.25. Another situation in which the robot has to perform quite a complex maneuver in order to move very little

Given that w = −grad(V (p)), we can deduce that: w (p, t) = −grad V (p) = −

"

dV (p) dp

#T

=v ˆ − 2 (p − p ˆ) +

(p − q ˆ)

3.

-p − q ˆ-

Guidance

213

2) For the control, we can take a proportional speed and heading control, which gives us: u=

"

-w (p, t)- − v sawtooth (angle (w (p, t)) − θ)

#

where the sawtooth function avoids the jumps of 2π. Given that the target is p ˆ = (t, t), the target speed has to be taken as v ˆ = (1, 1). 3) Let us take the vector field: w=v ¯−

p−p ˆ p−q ˆ dˆ p 1 + ¯= = 3 with v 2 dt 10 -p − q ˆ-

"

t − sin 10 t 2 cos 10

#

which gives an acceptable behavior. We have simply reduced the proportional term (which corresponds to the attractiveness of the target) in order not to be too sensitive to the variations of the vector field around the target, which would cause violent and undesirable changes in heading. C ORRECTION FOR E XERCISE 4.8.– (Flocking) Define the potential of the ith robot as Vi =

1 j-=i

2

α -p(i) − p(j)- +

β -p(i) − p(j)-

where p(i) = (xi , yj ). The quadratic part of the sum corresponds to the cohesion, and the hyperbolic term corresponds to the separation. The coefficients α and β have to be tuned. For instance, if β is high, then the repulsion will be stronger and the flock will be more scattered. The gradient of Vi is > ? T 1 dVi β (p(i) − p(j)) = 2α (p(i) − p(j)) − . 3 dp(i) -p(i) − p(j)j-=i Since we want to decrease Vi , we have to follow the opposite direction of the gradient. Moreover, in order to follow Reynolds’ alignment rule, the robot should get the same heading as the other robots. Thus, the controller is built to follow the direction > " #? T 1 β (p(i) − p(j)) cos θj −2α (p(i) − p(j)) − . 3 + sin θj -p(i) − p(j)j-=i

214

Mobile Robotics

C ORRECTION FOR E XERCISE 4.9.– (Platooning) 1) The observation equation is given as: . .? " # > a −a di r · sawtooth i−r i − π + π y(i) = = . d˙i v i− − v i The sawtooth function is used to get a positive circular distance. 2) We propose the control: ui = (di − d0 ) + d˙i + (v0 − vi ) = y1 (i) − d0 + y2 (i) + v0 − vi where d0 = L/m. After few seconds, the robots form a uniform distribution and move at a speed equal to v0 . 3) Take as state variables (d1 , d2 , · · · , dm−1 , v1 , v2 , v3 , · · · , vm ). Note that we do not need dm as a state variable since d1 + · · · + dm = L.  d˙1    ˙  d2     ..    .    ˙m−1  d      v˙ 1    v˙ 2        ..    .     v˙ m   

= = .. .

vm − v1 v1 − v2 .. .

= vm−2 − vm−1 = u1 = = u2

=

.. . =

.. . =

.. . um =

d1 − d0 + d˙1 + v0 − v1 2345 vm −v1

d2 − d0 + d˙2 + v0 − v2 2345 v1 −v2

L−d1 −···−dm−1

If the system is stable, we obtain  0     0     .    ..    0   0 0      ..   .     0     L   L

dm 2 34 5

= = .. .

vm − v1 v1 − v2 .. .

= = = .. .

vm−2 − vm−1 d1 + vm − 2v1 + v0 − d0 d2 + v1 − 2v2 + v0 − d0 .. .

− d0 +

d˙m 2345

vm−1 −vm

= −d1 − · · · − dm−1 + vm−1 − 2vm + v0 − d0 + L = d1 + · · · + dm = md0

+ v0 − vm

Guidance

215

We have 2m − 1 equations. The first m − 1 equations give us that v1 = v2 = · · · = vm . The following m − 1 equations tell us that d1 = · · · = dm−1 . We get  v1 = v2 = · · · = vm     d  1 = · · · = dm−1   d1 − v1 + v0 − d0 = 0 − (m − 1) d1 − v1 + v0 − d0 + L = 0     L = (m − 1) d1 + dm    L = md0

We easily get !

v1 = v2 = · · · = vm = v0 d0 = d1 = · · · = dm−1 = dm =

L m

4) For m = 4, we have 

  d1 0  d2   0     d3   0   d   v1  =  1   dt      v2   0  v3   0 v4 −1

0 0 0 0 1 0 −1

0 0 0 0 0 1 −1

−1 1 0 −2 1 0 0

0 −1 1 0 −2 1 0

0 0 −1 0 0 −2 1

    1 d1 0     0  0   d2     d3    1  0      v1  +  v0 − d0  1          0    v2   v0 − d0  0   v3   v0 − d0  −2 v4 v0 − d0 + L

The eigenvalues of the evolution matrix are {−2, −1 + i, −1 − i, −1, −1, −1, −1}. Therefore, we have the stability of the system. 5) Figure 4.26 illustrates the behavior of the controller for 10 robots. For the initialization (a), we created a gap that the red car and followers have to fill. The front cars accelerate at (b), but decelerate at (d) for more stability in order to avoid oscillations in the group. At (f ), the group is uniformly distributed in the circle and all cars go at the right velocity. C ORRECTION FOR E XERCISE 4.10.– (Action on a vector field) 1) In a time dt, x(t) has moved dx = dt · f (x(t)). We have y(t + dt) = = = = =

g(x(t) + dx) dg g(x(t)) + dx (x(t)) · dx dg g(x(t)) + dx (x(t)) · dt · f (x(t)) dg y(t) + dt · dx (g−1 (y(t))) · f (g−1 (y(t))) y(t) + dt · (g • f ) (y(t)).

216

Mobile Robotics

Figure 4.26. (a) initialization at time t = 0; (b)–(f ) the controller yields a uniform distribution between cars. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

/ 0 2) The function −r r2 − 1 has three roots:−1, 0, 1. It is positive if r ∈ [0, 1] and negative if r ≥ 1. Thus, r(t) will converge to 1. To prove this, we build the function 2 V (r) = (r − 1) . We have 0 / 2 V˙ = 2 (r − 1) r˙ = −2 (r − 1) r r2 − 1 = −2 (r − 1) r (r + 1) .

Since r > 0, we have V˙ < 0 except for r = 1 where V˙ = 0. The function V is a Lyapunov function and thus V (r) will converge to 0. Equivalently, r converges to 1. In the polar plane, for all initial vectors, the trajectory will converge counterclockwise to the circle of radius r = 1. 3) We rewrite the state equation into p˙ = h(p), where p = (r, θ). We have dg = dp

"

cos θ −r sin θ sin θ r cos θ

#

.

Thus -

. / 0 ◦ g−1 (x) · h ◦ g−1 (x)  " : / 2 0# √ x21 2 −x2 2 2 2 x +x  · − x1 + x2 x1 + x2 − 1 =  √ x12 2 x1 1 2 2 " x31 +x2 2 # −x1 − x1 x2 + x1 − x2 = −x32 − x21 x2 + x1 + x2

(g • h) (x) =

dg dp

Guidance

217

4) If g(x) = Ax, we get "

# dg / −1 0 (g • f ) (x) = g (x) · f (g−1 (x)) dx 2 34 5 A

= A · f (A−1 · x).

5) The vector field is given by fA (x) = A · f (A−1 · x) where A=

"

#" #" # cos π4 − sin π4 2 0 1 0 sin π4 cos π4 0 1 0 −1 2 34 52 34 52 34 5 R

D

S

The first matrix S changes the orientation of the field to get a clockwise rotation of the trajectories, D gives the right dimensions of the limit ellipse and R rotates the field to get the right inclination. Figure 4.27 depicts the fields f (x),fS (x),fDS (x) and fA (x) respectively. C ORRECTION FOR E XERCISE 4.11.– (Eight circuit) 1) We consider the action g(p) =

"

ρ 0 0 ρ

# " # " # 1 0 ρ 0 · ·p+c= ·p+c 0 ε 0 ρε 2 34 5 D

Φε,ρ,c = (g - • Φ0 ) (p) . / 0 dg = dp ◦ g−1 (p) · Φ0 ◦ g−1 (p) / 0 = D · Φ0 D−1 · (p − c) .

2) To follow the field Φ = Φε,ρ,c , when the robot is at position p = (x1 , x2 ), we choose for the error y = x3 − atan2(Φ2 (p), Φ1 (p)). 2 34 5 2 34 5 b

a

218

Mobile Robotics

Figure 4.27. Generation of a vector field with a stable limit cycle corresponding to a specific ellipse. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

We have b a ˙ y˙ = x˙ 3 − (− 2 · a˙ + 2 · b) a + b2 a + b2 2 34 5 2 34 5 ∂atan2(b,a) ∂a

= u+

b·a−a· ˙ b˙ a2 +b2

∂atan2(b,a) ∂b

where " # a˙ d = dt Φε,ρ,c b˙ 00 / / d = dt D ·/Φ0 /D−1 · (p − c)00 d Φ0 D−1 · (p − c). = D · -dt 0 0 / −1 / −1 d 0 D · (p − c) · dt D · (p − c) = D · dΦ dp 0. / −1 0 D · (p − c) · D−1 · p˙ = D · dΦ dp " ### # "" # " " x1 cos x3 dΦ0 −1 −1 −c ·D · = D · dp D · x2 sin x3

Guidance

219

and dΦ0 d = dp dp

"

−p31 − p1 p22 + p1 − p2 −p32 − p21 p2 + p1 + p2

#

=

"

−3p21 − p22 + 1 −2p1 p2 − 1 −2p1 p2 + 1 −3p22 − p21 + 1

#

.

If we want for the dynamic of the error, y˙ = −y,we take u = −y −

˙ b˙ ) (b·a−a· . 2 2 a +b

The controller is thus uε,ρ,c (x) = −sawtooth (x3 − atan2(b, a)) −

˙ b˙ ) (b·a−a· a2 +b2

with " # " # a˙ cos x3 −1 = D · J · D · sin x3 b˙ " # −3z12 − z22 + 1 −2z1 z2 − 1 J = 2 2 −2z 1 −3z 1 z2 +# # 2 − z1 + 1 "" x1 −c z = D−1 · " # x2 ρ 0 D = 0 ρε 3) We built two distinct sets: Q1 = {p|p2 ≥ 1} Q2 = {p|p2 ≤ 0} It is important to have these sets distinct to avoid Zeno effects, i.e. switching from one mode to another an infinite number of times, all within a finite interval of time. Then, we build the automaton as on Figure 4.28. The resulting behavior is illustrated by Figure 4.29.

220

Mobile Robotics

Figure 4.28. Automaton switching from one field to the other. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

Figure 4.29. Top: the two vector fields; bottom left: A superposition of the two fields with the two transition sets Q1 , Q2 ; Bottom right: the resulting trajectory forms an eight. For a color version of this figure, see www.iste.co.uk/jaulin/robotics.zip

5 Instantaneous Localization

Localization consists of finding the position of the robot (i.e. the coordinates of its center as well as its orientation), or more generally all its degrees of freedom. This problem is encountered in navigation, where we need to approximate the position, orientation and speed of the robot. The problem of localization is often considered to be a particular case of state estimation, which will be presented in the following chapters. However, in the case where an accurate state model is not available for our robot, an instantaneous localization often remains possible and may be sufficient for making a decision. Let us take, for instance, the situation in which we are aboard a ship and have just detected a lighthouse, whose absolute position and height are known. By measuring the perceived height of the lighthouse and its angle relative to the ship, we may deduce the position of the ship using a compass and this, without using a state model for the ship. Instantaneous, or model-free localization is an approach to localization that does not utilize the evolution equation of the robot, i.e. it does not seek to make the measures coherent through time. This localization mainly consists of solving equations of geometric nature which are often nonlinear. The variables involved may be position variables or kinematic variables such as speed or accelerations. Since these localization methods are specific and quite far from state estimation methods, we will devote an entire chapter to them. After introducing the main sensors used for localization, we will present goniometric localization (in which the robot uses the angles of perception of landmarks) followed by multilateration, which uses distances between the robot and the landmarks. 5.1. Sensors The robots are equipped with numerous sensors that are used for their localization. We will now present some of these. Odometers. Robots with wheels are generally equipped with odometers that measure the angular movements of the wheels. Given only the odometers, it is Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

222

Mobile Robotics

possible to calculate an estimation of the position of the robot. The precision of such a localization is very low, given the systematic integration of the estimation error. We say that the estimation is drifting. Doppler log. This type of sensor, mainly used in underwater robotics, allows to calculate the speed of the robot. A Doppler log emits ultrasounds that are reflected by the ocean bed. Since the ocean bed is immobile, the sensor is able to estimate the speed of the robot by using the Doppler effect with a very high precision (around 0.1 m/s). Accelerometers. These sensors provide measurements of the instantaneous forward acceleration. The principle of the axis-based accelerometer is illustrated on Figure 5.1. Generally, three accelerometers are used by the robot. Due to gravity, the value measured according to the vertical axis must be compensated.

Figure 5.1. Operating principle of an accelerometer

Gyro or gyroscope. These sensors provide measurements of the instantaneous rotation speed. There are three types of gyros: the Coriolis vibratory gyro, the mechanical gyro and the optical gyro. The principle of the Coriolis vibratory gyro is illustrated on the left of Figure 5.2. A vertical rod placed on a horizontal disk vibrates from left to right. As a result of the Coriolis force, if the disk rotates, there is an angular vibration following the axis of the rod whose amplitude allows to get the rotation speed of the disk. If the disk is not rotating, there is a forward rotation, but it is not angular. Piezoelectric gyros, very widely used for low-cost robotics, form a subclass of Coriolis vibratory gyroscopes. These gyros exploit the variation of the amplitude of a piezoelectric oscillator induced by the Coriolis force, due to the rotation applied to the sensor. Mechanical gyros make use of the fact that a rotating body tends to preserve its rotational axis if no torque is subjected to it. A well-known example is the gimbal gyroscope invented by Foucault, represented on the right side of Figure 5.2. A flywheel at the center rotates with high speed. If the base of the gyroscope moves, the two gimbal angles ψ, θ will change, but the rotation axis of the ˙ θ, ˙ we can find the rotation speed of the flywheel will not. From the values of ψ, θ, ψ, base (which is fixed on the robot). If the rotation axis of the flywheel is initialized

Instantaneous Localization

223

correctly, and in a perfect situation in which no torque is exerted on this flywheel, such a system would theoretically give us the orientation of the robot. Unfortunately, there is always a small drift and only the rotation speed can be given in a reliable and drift-free way.

Figure 5.2. Coriolis vibratory gyroscope and gimbal gyroscope

More recent, optical gyroscopes can be as precise as mechanical ones. They make use of the Sagnac effect (for a circular optical path, the time taken by light to make a complete lap depends on the direction of the path) and have a precision of around 0.001 deg/s. Their principle is illustrated in Figure 5.3. On figure (a), the laser leaves the light source represented by the black disk. On figure (b), the beam splitter creates two other beams which travel in opposite directions in the optical loop. After rebounding several times on the three mirrors represented in grey, the two beams meet. Since the beams intersect on the left, the gyro rotates in the opposite trigonometric direction (c). The beams are separated again on figure (d). The two beams that arrive at the receiver are not in phase. Their phase offset allows to find the rotation speed of the gyro, which is fixed on the robot. Inertial unit. An inertial measurement unit associates a gyro and an accelerometer in order to increase the precision of the estimation. More recent ones merge other types of information such as the estimated speed or even take into account Earth’s rotation. For instance, the Octans III unit of the IXBLUE company uses the Sagnac effect together with Earth’s rotation, in order to deduce the direction of the Earth’s North-South axis in the robot’s coordinate system. Knowing this direction gives us two equations involving the Euler angles of the robot (see section 1.2) which are the bank φ, the elevation θ and the heading ψ of the robot, expressed in a local coordinate system. Due to the accelerometer included in the unit, it is possible to deduce the gravity vector from the above and thus to generate an additional equation which will allow to calculate the three Euler angles. Let us note that the accelerometers also

224

Mobile Robotics

give us the accelerations in all directions (the surge in the direction of the robot, the heave (in the vertical direction), the sway for lateral accelerations). The knowledge of the gravity vector and the axis of rotation theoretically allows, using a simple scalar product, to find the latitude of the robot. However, the obtained precision is too low to be taken into account in localization.

Figure 5.3. Principle of the Sagnac effect for optical gyroscopes

Barometer. This measures pressure. In the case of underwater robots, it allows to deduce the depth of the robot with a precision of 1 cm. For indoor flying robots, the barometer is used to measure the altitude with a precision of around one meter. GPS (Global Positioning System). The GNSS (Global Navigation Satellite System) is a satellite navigation system that provides a geolocalization service covering the entire world. Nowadays, the American NAVSTAR system (NAVigation System by Timing And Ranging) and the Russian GLONASS system (GLObalnaya NAvigazionnaya Sputnikovaya Sistema) are operational. Two other systems are being developed: the Chinese Compass system and the European Galileo system. In practice, our mobile robots will use the American system, operational since 1995, that we will refer to as GPS. Originally designed for exclusive military use, the precision of civil applications was limited to several hundreds of meters by a deliberate degradation of civil signals. The deactivation of this degradation in 2000 allowed the precision to increase to about ten meters. Given that electromagnetic waves (here around 1.2 MHz) do not propagate under water or across walls, the GPS does not function within buildings or in water. Thus, during a diving experiment, an underwater robot can only be localized by GPS when it begins its dive or when it resurfaces. When a georeferenced station is near the robot and advises it about the errors in distance calculated for each satellite, a localization with a precision of ±1 m is possible. This operating mode forms the so-called differential GPS or DGPS. Finally, by using the phase, it us possible to achieve even a centimeter precision. This is the principle of the kinematic GPS. A detailed and educational presentation of the GPS can be found in the thesis of Vincent Drevelle [DRE 11]. In practice, a GPS gives us a longitude *x and a latitude *y and it is often comfortable to convert it to

Instantaneous Localization

225

Cartesian coordinates in a local coordinate system (o, i, j, k) fixed within the area in which the robot is evolving. Let us denote by *0x and *0y the longitude and the latitude expressed in radians at the origin o of this coordinate system. We will assume that the vector i indicates the North, j indicates the East and k is oriented towards the center of the Earth. Let p = (px , py , pz ) be the coordinates of the robot expressed in the coordinate system (o, i, j, k). From the latitude and longitude given by the GPS, we can deduce the first two coordinates of the robot, expressed in meters in the local coordinate system, by using the following relation (see equation [4.4]): "

px py

#

=ρ

"

0 cos *y

1 0

#"

*x − *0x *y − *0y

#

=

"

/ 0 # ρ *y /− *0y 0 ρ cos *y · *x − *0x

where ρ corresponds to the distance between o and the center of the Earth (ρ . 6 371 km, if o is not too far from sea level). This formula is valid everywhere on Earth, if we assume that the Earth is spherical and if the robot is in the neighborhood of the origin o (let us say a distance inferior to 100 km). In order to understand this formula, we must note that ρ cos (*y ) corresponds to the distance between o and the rotational axis of the Earth. Thus, if a robot is moving on a latitude parallel *y , by modifying its longitude by an angle α > 0, it will have traveled αρ cos *y meters. Similarly, if this robot is moving on a meridian with an angle β in latitude, it will have traveled βρ meters. Radar or sonar. The robot emits electromagnetic or ultrasound waves. It recovers their echoes and builds an image that it interprets in order to map its surroundings. The radar is mainly used by surface or flying robots. The sonar is used as a low-cost rangefinder by robots with wheels, as well as in underwater robotics. Cameras. Cameras are low-cost sensors used for the recognition of objects. They are also used as goniometers by finding angles relative to landmarks that will then be used for localization. 5.2. Goniometric localization 5.2.1. Formulation of the problem The problem consists of using angles measured between the robot and the landmarks, whose position as a function of time is known, for localization. Let us consider the robot in Figure 5.4 moving on a plane. We call bearing the angle αi between the axis of the robot and the vector pointing towards the landmark. These angles could have been obtained, for instance, using a camera.

226

Mobile Robotics

Figure 5.4. A robot moving on a plane, measures the angles in order to locate itself

Recall that two vectors u, v of R2 are collinear if their determinant is zero, i.e. if det (u, v) = 0. Thus, for each landmark, we have the relation: det

""

xi − x yi − y

# " ## cos (θ + αi ) , = 0, sin (θ + αi )

i.e. (xi − x) sin (θ + αi ) − (yi − y) cos (θ + αi ) = 0,

[5.1]

where (xi , yi ) are the coordinates of the landmark ai and θ is the robot’s heading. 5.2.2. Inscribed angles Inscribed Angle Theorem: Consider triangle abm as represented on Figure 5.5. Let us denote by c the center of the circle circumscribed to this triangle (i.e. c is at the M ! γ = acb. intersection of the three perpendicular bisectors). Let α = a! mc, β = cmb, We have the angular relation: γ = 2 (α + β) . P ROOF.– Since the two triangles amc and cmb are isosceles. We thus find the angles α and β as shown on the figure. By going around the point c, we obtain: γ + (π − 2β) + (π − 2α) = 2π

Instantaneous Localization

227

Figure 5.5. Illustration of the Inscribed Angle Theorem

Thus γ = 2α + 2β. ! A consequence of this theorem is that if m moves on the circle, the angle α + β will not move. Inscribed arcs. Let us consider two points, a1 and a2 . The set of points m are such that the angle a" 1 ma2 is equal to α is a circle arc, referred to as an inscribed arc. We can show this from relations [5.1] or from the Inscribed Angle Theorem. Goniometric localization often breaks down to intersecting arcs. Figure 5.6 illustrates the concept of an inscribed arc.

Figure 5.6. The three cars perceive the landmarks with the same angle

228

Mobile Robotics

5.2.3. Static triangulation of a plane robot 5.2.3.1. Two landmarks and a compass In the case where we have two landmarks and a compass, we have, following [5.1], the two relations: ! (x1 − x) sin (θ + α1 ) − (y1 − y) cos (θ + α1 ) = 0 (x2 − x) sin (θ + α2 ) − (y2 − y) cos (θ + α2 ) = 0 Or equivalently "

#" # " # sin (θ + α1 ) − cos (θ + α1 ) x x1 sin (θ + α1 ) − y1 cos (θ + α1 ) = sin (θ + α2 ) − cos (θ + α2 ) y x2 sin (θ + α2 ) − y2 cos (θ + α2 ) 2 34 5 2 34 5 A(θ,α1 ,α2 )

b(θ,α1 ,α2 ,x1 ,y1 ,x2 ,y2 )

i.e.

" # x = A−1 (θ, α1 , α2 ) · b (θ, α1 , α2 , x1 , y1 , x2 , y2 ) . y The problem of localization is therefore a linear one, which can be solved analytically. We have an identifiability problem if the matrix to invert has zero determinant, i.e. sin (θ + α1 ) cos (θ + α2 ) = cos (θ + α1 ) sin (θ + α2 ) ⇔ tan (θ + α2 ) = tan (θ + α1 ) ⇔ θ + α2 = θ + α1 + kπ, k ∈ N ⇔ α2 = α1 + kπ, k ∈ N This corresponds to a situation in which the two landmarks and the robot are aligned. 5.2.3.2. Three landmarks If we no longer have a compass, we need at least three landmarks. We then need to solve the system of three equations and three unknowns: (xi − x) sin (θ + αi ) − (yi − y) cos (θ + αi ) = 0, i ∈ {1, 2, 3}. It can be shown that this system always has a unique solution, except when the robot is located on the circle that passes through all three landmarks. Indeed, in such a case the inscribed angles are superimposed.

Instantaneous Localization

229

5.2.4. Dynamic triangulation 5.2.4.1. One landmark, a compass, several odometers In the case of dynamic state observation, we are looking for the relation that connects the position of the robot to the derivatives of the values measured. For localization, we will assume that a single landmark is available to us. We will use the equations: ! x˙ = v cos θ [5.2] y˙ = v sin θ

where v represents the speed of the robot measured by the odometer and θ its heading measured by the compass. These equations, which are kinematic in nature, are not supposed to describe the behavior of a particular robot with the aim of controlling it. The inputs v and θ are not necessarily the real inputs of the system that we can act on. These equations have to be understood as a simple differential relation between the variables of a plane robot. By differentiating relation [5.1], we obtain: . (x˙ i − x) ˙ sin (θ + αi ) + (xi − x) θ˙ + α˙ i cos (θ + αi ) . [5.3] − (y˙ i − y) ˙ cos (θ + αi ) + (yi − y) θ˙ + α˙ i sin (θ + αi ) = 0 Let us take the relations [5.1] and [5.3] for i = 1. By isolating x and y, we obtain: " # " # x sin (θ + α1 ) cos (θ + α1 ) = y − >cos (θ + α1 ) sin (θ + α1 ) ?" # [5.4] −y1 x1 cos (θ + α1 ) · −v sin θ x˙ 1 −v cos θ x1 − y˙ 1 θ+ + y1 sin (θ + α1 ) ˙ α ˙ α ˙ θ+ ˙ 1

1

This relation can allow us to be located using a single mobile or fixed landmark and other proprioceptive sensors. For instance, in the case where we have a compass and several odometers (for a robot on wheels), we are able to measure the heading θ using the compass, the speed v and θ˙ using the odometers. relation [5.4] then allows us to calculate the position x and y at the given moment in time. 5.2.4.2. One landmark, no compass In the situation where the compass is not present, we are missing an equation. We either need to add a second landmark, or differentiate again. Let us remain with a single landmark and differentiate relation [5.3], we obtain: (¨ x1 − x ¨) sin-(θ + α1.) − (¨ y1 − y¨) cos (θ + α1 ) . ¨ + (x1 − x) θ + α ¨ 1 cos (θ + α1 ) + (y1 − y) θ¨ + α ¨ 1 sin (θ + α1 ) . . +2 (x˙ 1 − x) ˙ θ˙ + α˙ 1 cos (θ + α1 ) + 2 (y˙ 1 − y) ˙ θ˙ + α˙ 1 sin (θ + α1 ) .2 .2 − (x1 − x) θ˙ + α˙ 1 sin (θ + α1 ) + (y1 − y) θ˙ + α˙ 1 cos (θ + α1 ) = 0

230

Mobile Robotics

Moreover: !

x ¨ = v˙ cos θ − v θ˙ sin θ y¨ = v˙ sin θ + v θ˙ cos θ

We thus obtain a system of three equations with three unknowns x, y, θ: (x1 − x) sin (θ + α1 ) − (y1 − y) cos (θ + α1 ) = 0 . (x˙ 1 − v cos θ) sin (θ + α1 ) + (x1 − x) θ˙ + α˙ 1 cos (θ + α1 ) . − (y˙ 1 − v sin θ) cos (θ + α1 ) + (y1 − y) θ˙ + α˙ 1 sin (θ + α1 ) = 0 . . x ¨1 − v˙ cos θ + v θ˙ sin θ sin (θ + α1 ) − y¨1 − v˙ sin θ − v θ˙ cos θ cos (θ + α1 ) . . + (x1 − x) θ¨ + α ¨ 1 cos (θ + α1 ) + (y1 − y) θ¨ + α ¨ 1 sin (θ + α1 ) . . +2 (x˙ 1 − v cos θ) θ˙ + α˙ 1 cos (θ + α1 ) + 2 (y˙ 1 − v sin θ) θ˙ + α˙ 1 sin (θ + α1 ) .2 .2 − (x1 − x) θ˙ + α˙ 1 sin (θ + α1 ) + (y1 − y) θ˙ + α˙ 1 cos (θ + α1 ) = 0

The quantities x1 , y1 , x˙ 1 , y˙ 1 , x ¨1 , y¨1 are calculated from the path of landmark 1, for which we know an analytic expression. The quantities α1 , α˙ 1 , α ¨ 1 are assumed to ˙ θ¨ can be obtained using a gyro. The speed v can be be measured. The quantities θ, measured using odometers. It is clear that this system is not easy to solve analytically and does not always admit a unique solution. For instance, if the landmark is fixed, by rotational symmetry we can see that we will not be able to find the angle θ. In such a case, we need at least two landmarks for localization. 5.3. Multilateration

Multilateration is a localization technique based on measuring the difference of the distances between the robot and the landmarks. Indeed, in a number of situations (such as in GPS localization), the clocks between the landmarks and the robot are not synchronized and we can not, therefore, directly measure the absolute distance between the landmarks and the robot (by the propagation time of air- or soundwaves), but we can measure the difference between these distances. We will now give the principles of this technique. Four landmarks emit a brief signal at the same time t0 which propagates with a speed c. Each emitted signal contains the identifier of the landmark, its position and the emission time t0 . The robot (which does not have an accurate clock, only an accurate chronometer) receives the four signals at times ti . From this it easily deduces

Instantaneous Localization

231

the offsets between the reception times τ2 = t2 − t1 , τ3 = t3 − t1 , τ4 = t4 − t1 (see Figure 5.7). We thus obtain the four equations: < 2 2 2 (x − x1 ) + (y − y1 ) + (z − z1 ) = c(t1 − t0 ) < 2 2 2 (x − x2 ) + (y − y2 ) + (z − z2 ) = c(τ2 + t1 − t0 ) < 2 2 2 (x − x3 ) + (y − y3 ) + (z − z3 ) = c(τ3 + t1 − t0 ) < 2 2 2 (x − x4 ) + (y − y4 ) + (z − z4 ) = c(τ4 + t1 − t0 )

where the parameters, whose values are known with high precision, are c,t0 ,x1 , y1 , z1 , . . . ,x4 , y4 , z4 , τ2 ,τ3 ,τ4 . The four unknowns are x, y, z, t1 . Solving this system allows it to be localized and also to readjust its clock (through t1 ). In the case of the GPS, the landmarks are mobile. They use a similar principle to be localized and synchronized, from fixed landmarks on the ground.

Figure 5.7. The emission time t0 and the offsets between the arrival times τ2 , τ3 , τ4 are known

5.4. Exercises E XERCISE 5.1.– Instantaneous state estimation Localization consists of finding the position and the orientation of the robot. This problem can sometimes be reduced to a state estimation problem, if the state model for our robot is available. In this exercise, we will give a method that is sometimes used for the state estimation of nonlinear systems. Let us consider the tricycle given in section 2.5, described by the state equations:     x˙ v cos δ cos θ  y˙   v cos δ sin θ       θ˙  =  v sin δ        v˙   u1 ˙δ u2 We measure the positions x and y with such high precision that we may assume that x, ˙ y, ˙ x ¨, y¨ are known. Express the other state variables θ, v, δ in function of x, y, x, ˙ y, ˙ x ¨, y¨.

232

Mobile Robotics

E XERCISE 5.2.– Localization by lidar Here we are interested in developing a fast localization method for a robot using a rotating laser rangefinder, or lidar (light radar) of type Hokuyo, in a rectangular room whose length and width are unknown. 1) Let a1 , . . . , anp be points of R2 located on the same line. Find this line using a least squares method. Represent this line in normal form: x cos α + y sin α = d, with d ≥ 0 where α, d are the parameters of the line. 2) Consider one hundred measurements θi of the same angle θ of a robot by one hundred compasses placed on the robots, of which 30 are defective. Estimate θ using the median method. For this, we define the disambiguation function:  2 R → R " # cos θ f: θ → sin θ

Such a function, which must be continuous, aims to associate a single image to two equivalent angles (i.e. congruent to 2π), i.e. θ1 ∼ θ2 ⇔ f (θ1 ) = f (θ2 ) . 3) The robot’s lidar, which has an aperture angle of 180 degrees, gives us 512 points that belong to the rectangle representing our room. These points can be found in the file lidar_data. We will take them in groups of ten (i.e. 51 groups) and try to find the line that passes the best through each group (using the least squares method). We will only keep the groups with a small residue. We thus obtain m lines, represented by n points of the form (αi , di ) , i ∈ {1, . . . , m} in the so-called Hough space. A pair (αi , di ) is called an alignment. By using a median estimator, find the four possible directions for our room (knowing that it is rectangular). Why is this median estimator considered robust? 4) How are the angles αi filtered from the alignments? 5) How are the di filtered? 6) Deduce from the above a method for localizing the robot.

Instantaneous Localization

233

E XERCISE 5.3.– Instantaneous goniometric localization Consider a robot boat R described by the state equations:    x˙  y˙ θ˙    v˙

= = = =

v cos θ v sin θ u1 u2

where v is the speed of the robot, θ its orientation and (x, y) the coordinates of its center. Its state vector is given by x = (x, y, θ, v) . In the surroundings of the robot there is a point landmark (a lighthouse for instance) m = (xm , ym ) whose position is known (see Figure 5.8). The robot R is equipped with five sensors: an omnidirectional camera (allowing it to measure the bearing angle α of the landmark’s direction), odometers for measuring its speed, a compass that gives its heading θ, an accelerometer for u2 and a gyro for u1 .

Figure 5.8. Goniometric localization

1) Simulate this system in a situation where the robot is moving around m in order to generate the signals α, α, ˙ θ, v, u1 , u2 . For generating α, use the two-argument arctangent function atan2. As we have seen in equation [1.10], the atan2(b, a) function returns the argument of the coordinate vector (a, b). For generating α˙ we could use the fact that: ∂atan2 (b, a) b =− 2 ∂a a + b2

and

∂atan2 (b, a) a = 2 ∂b a + b2

234

Mobile Robotics

2) Design an instantaneous localization system for R. Verify this localization system using a simulation. Add a small white Gaussian noise in your measurements to test its robustness. 3) The robot R is no longer equipped with odometers. How would you adjust the previous approach to allow localization? 4) Let us add a second robot Rb to the scene which is of the same type as the first (which we will now refer to as Ra ), with the exception that it has no odometers, contrarily to Ra . Robot Rb can see Ra (see Figure 5.9) which gives it an angle βb . Both robots can communicate via WIFI. Design a localization system for the robot Rb .

Figure 5.9. The robots Ra and Ra can see each other, which will aid them in the localization

5) Robot Ra can see Rb , which gives it a new angular measurement βa . Deduce a localization method from this for Ra and Rb that is more reliable (i.e. one that presents less singularities) than that developed in the first question. E XERCISE 5.4.– Localization by distance measuring We consider a robot described by the state equations:  x˙ = v cos θ    y˙ = v sin θ θ˙ = u1    v˙ = u2

where v is the speed of the robot, θ its orientation and (x, y) the coordinates of its center c. Its state vector is given by x = (x, y, θ, v).

Instantaneous Localization

235

In the surroundings of the robot there is a fixed point landmark m whose position is known (see Figure 5.10). Each instant, the robot measures the distance d between its center c and the landmark. If the landmark and the robot both have synchronized clocks, such a system for measuring the distance can be done by measuring the propagation time of a sound wave between the landmark and the robot. Moreover, by using the Doppler effect, the robot is also capable of measuring d˙ with very high precision. In addition to the microphone that allows it to measure d and d,˙ the robot is equipped with odometers to measure its speed v, a compass that gives its heading θ. In this exercise, we are looking to build an instantaneous localization system in order ˙ θ, v. to determine its position (x, y) from d, d, . ˙ θ, v , there may be several configurations possible for 1) For a given vector d, d, the robot. Draw, on the picture, all the configurations for the robot that are compatible with the one that is represented. 2) Consider the coordinate system R1 centered at m as represented on the figure. Give the expression of the coordinates (x1 , y1 ) of the center c of the robot in function of x, y, θ, xm , ym . This is in fact a coordinate system change equation. ˙ v. 3) Express x1 and y1 in function of d, d, ˙ θ, v, 4) From this, deduce the position(s) of the robot (x, y) in function of (d, d, xm , ym ). What are the singularities of this localization system? In which case do we have a single solution?

Figure 5.10. The robot measures the distance to the landmark

236

Mobile Robotics

5.5. Corrections C ORRECTION FOR E XERCISE 5.1.– (Instantaneous state estimation) We have: θ = arctan

" # y˙ . x˙

By differentiating this equation, we obtain: 1 θ˙ = - .2 y˙ x˙

+1

"

y¨x˙ − x ¨y˙ x˙ 2

#

.

Moreover, following the state equations of the tricycle:   v cos δ =

x˙ cos θ  v sin δ = θ˙

By isolating v and δ, we obtain:  ; <  x˙ 2 + θ˙ 2   v = (v cos δ)2 + (v sin δ)2 = 2 cos θ " # ˙  θ   δ = arctan x ˙ cos θ

Thus, the state vector is expressed as an analytic function of the output (x, y) and of its derivatives:   x   y- .       y ˙ x arctan x˙   y  L  .2     θ  =   y¨x−¨ ˙ xy˙ x˙ 2 1    cos2 (arctan( y˙ )) + * y˙ 2 +2  x˙ 2 x ˙  ( x˙ ) +1 v      1  y ¨x−¨ ˙ xy˙   δ ( x˙ 2 ) y˙ 2   +1 ( ) x ˙     arctan x ˙ y˙ cos(arctan( )) x ˙

We thus have a state observer that we may describe as quasi-static since, contrarily to the standard observers, this one does not require the integration of differential equations (they are not dynamic). Obtaining such an observer can be

Instantaneous Localization

237

done for a large class of nonlinear systems encompassing the class of flat systems [LAR 03]. C ORRECTION FOR E XERCISE 5.2.– (Localization by lidar) 1) For the line equations, there is the explicit form y = ax + b, the standard form ax+by+c = 0, the normal form x cos α+y sin α = d, as well as other forms which we will not talk about here. The explicit form can represent vertical lines and, therefore, this form contains singularities. The standard form does not have any singularities. However the associated model is non-identifiable (since different sets of parameters may lead to the same line). The normal form is identifiable, without singularities but it is nonlinear. Let us take the form p1 x + p2 y = 1 (singular if the line passes through 0, which will not be the case for our robot). We have: p1 xi + p2 yi = 1, i.e. by considering that a small amount of noise is possible within the measurements: 

   y1 " # 1 ..  p1 .  ..  . . .  p2 xn yn 1 2 34 5 x1  ..  .

A

We thus take as estimator   1 / T 0−1 T  .  p ˆ= A A A  ..  . 2 34 5 1 K

However, we need an equation of the type x

cos α ˆ sin α ˆ +y = 1, with d ≥ 0. ˆ ˆ d d

We will therefore take: 1 dˆ = and α ˆ = angle (ˆ p) -ˆ pIn order to verify our calculations, let us generate 1,000 points that form an approximatively-aligned point cloud. Then we estimate the line. This can be done by the following program:

238

Mobile Robotics

1 2 3 4 5

˙ N := 1000; x := 5 · randn N ×1 y := −2x + 1N ×1 +/randnN 0 ×1 A := /(X, Y 0) ; K := AT A AT ; ˆ := AT A · AT · 1N ×1 p dˆ := &ˆp1 & ; α ˆ := angle (ˆ p)

2) We take the median x ˆ of the xi and yˆ of the yi . The angle estimator is θˆ = arg (ˆ x, yˆ) . To check the robustness of the estimator, we may use the following program

1 2 3 4 5

θ := 1; N = 100 − → θ := θ · 1N ×1 + 0.1 · randnN ×1 + round(randnN ×1 ); → − − → x := cos θ ; y = sin θ x ˆ := median(x); yˆ := median(y) θˆ := atan2(ˆ y, x ˆ)

3) Two angles α1 and α2 are equivalent if: α2 = α1 + kπ k∈Z 2 , ⇔ 4α = 4α + 2kπ, 2 1 ! cos 4α2 = cos 4α1 ⇔ sin 4α2 = sin 4α1 with k ∈ Z. At each alignment (αi , di ), we generate the m points of the unit circle: "

xi yi

#

=

"

cos 4αi sin 4αi

#

which represents the disambiguation function. Let us take the median x ˆ of the xi and the median yˆ of the yi . The argument of the vector (ˆ x, yˆ) gives us the angle 4ˆ α of the room (in the coordinate system of the robot) with a quarter turn margin. We will therefore take: α ˆ=

N π πO 1 atan2 (ˆ y, x ˆ) ∈ − , . 4 4 4

It is, of course, important at this point to remove the outliers in the orientation, i.e. the alignments (αi , di ) so that the quantity |ˆ x − xi | + |ˆ y − yi | is not negligible. 4) For each αi of the alignment (αi , di ), we calculate the wall number: # # " " αi − α ˆ ki = mod round , 4 ∈ {0, 1, 2, 3} π/4

Instantaneous Localization

239

The filtered angle is then given by: αi = α ˆ + ki

π 4

5) For each k ∈ {0, 1, 2, 3}, we calculate (using the median estimator) the distance: δˆk = median {di with i such that ki = k} 6) We define ηk as the number of alignments compatible with wall k, i.e. ηk = card({i | ki = k}). We, therefore, have knowledge of two or three distances δˆk corresponding to the orientation α ˆ + ki π4 and whose confidence index is given by ηk . In order to break the symmetry, we need additional information (such as the lengths and widths *x and *y of the rectangle, information about the heading). Figure 5.11 gives an illustration of the robot’s localization principle from the telemetric measurements.

Figure 5.11. Left: raw lidar data; middle: data associated with the small-residue alignments; right: recreated walls

C ORRECTION FOR E XERCISE 5.3.– (instantaneous goniometric localization) 1) For the simulation, we need α and α. ˙ First of all, we have: α = −θ + atan(ym − y, xm − x) By differentiating this relation, we obtain: d α˙ = −θ˙ + atan(ym − y, xm − x) dt " #" # − (ym − y) (xm − x) x˙ m − x˙ . = −θ˙ + 2 2 2 2 y˙ m − y˙ (xm − x) + (ym − y) (xm − x) + (ym − y)

240

Mobile Robotics

And therefore, since x˙ = v cos θ, y˙ = v sin θ and θ˙ = u1 , we have: α˙ = −u1 +

(xm − x) (y˙ m − v sin θ) − (ym − y) (x˙ m − v cos θ) 2

(xm − x) + (ym − y)

2

.

The code of the observation function is: ˙ Function g(x, u, m, m) ˙ 1 (x, y, θ, v) := x; (x˙ m , y˙ m ) := m 2 v := m − x1:2 ; α :=−θ+ angle(v) " # v1 x˙ m − v cos θ 1 3 α˙ := −u1 + &v& · det 2 v2 y˙ m − v sin θ T 4 Return (α, α, ˙ θ, v) 2) We have the relation: det

""

xm − x ym − y

# " ## cos (θ + α) , =0 sin (θ + α)

i.e.: (xm − x) sin (θ + α) − (ym − y) cos (θ + α) = 0 By differentiating this relation, we obtain: . (x˙ m − x) ˙ sin (θ + α) + (xm − x) θ˙ + α˙ cos (θ + α) . − (y˙ m − y) ˙ cos (θ + α) + (ym − y) θ˙ + α˙ sin (θ + α) = 0

Let us take the above relations. By isolating x and y, we obtain: " # " # x sin (θ + α) cos (θ + α) = y − cos (θ + α) sin (θ + α) > −ym y˙ − v sin θ v cos θ xm − m ˙ ym + x˙ m − θ + α˙ θ˙ + α˙

xm

? " # cos (θ + α) · sin (θ + α)

This relation can allow us to be located using a single landmark, (whether it is fixed or mobile) and other proprioceptive sensors. For instance, if we have a compass and odometers (for a robot in motion), we are capable of measuring the heading θa using the compass, the speed va and θ˙a using the odometers.

Instantaneous Localization

241

3) The expression of α ¨ would have to be calculated to have the missing equation. 4) We have two more equations available for Rb : ! (xm − xb ) sin (θb + αb ) − (ym − yb ) cos (θb + αb ) = 0 (xa − xb ) sin (θb + βb ) − (ya − yb ) cos (θb + βb ) = 0 which allow us to find (xb , yb ): "

xb yb

#

=

"

"

− sin (θb + αb ) cos (θb + αb ) − sin (θb + βb ) cos (θb + βb )

#−1

ym cos (θb + αb ) − xm sin (θb + αb ) ya cos (θb + βb ) − xa sin (θb + βb )

#

.

6) We have a new equation: (xa − xb ) sin (θa + βa ) − (ya − yb ) cos (θa + βa ) = 0 where βa is the angle with which Ra sees Rb . We thus have five equations with four unknowns: (xm − xa ) sin (θa + αa ) − (ym − ya ) cos (θa -+ αa ) = .0 (x˙ m − va cos θa ) sin (θa + αa ) + (xm − xa ) θ˙a + α˙ a cos (θa + αa ) . − (y˙ m − va sin θa ) cos (θa + αa ) + (ym − ya ) θ˙a + α˙ a sin (θa + αa ) = 0 (xm − xb ) sin (θb + αb ) − (ym − yb ) cos (θb + αb ) = 0 (xa − xb ) sin (θb + βb ) − (ya − yb ) cos (θb + βb ) = 0 (xa − xb ) sin (θa + βa ) − (ya − yb ) cos (θa + βa ) = 0 These equations are linear in (xa , ya , xb , yb ) and we can, therefore, solve them by the least squares formula. C ORRECTION FOR E XERCISE 5.4.– (Localization by distance measurement) 1) There are two solutions: the one already represented on Figure 5.10 and its symmetry with respect to the line that passes through m and with direction vector u = (cos θ, sin θ). 2) We have: " # " #" # x1 cos θ sin θ x − xm = . y1 − sin θ cos θ y − ym 3) We have: 2

2

(x − xm ) + (y − ym ) = d2 .

242

Mobile Robotics

˙ i.e. By differentiating, we obtain 2x˙ (x − xm ) + 2y˙ (y − ym ) = 2dd, ˙ (v cos θ) (x − xm ) + (v sin θ) (y − ym ) = dd. In the coordinate system R1 these two equations are written as: !

x21 + y12 = d2 ˙ vx1 = dd.

The resolution of this system is simple. We obtain: A

x1 =

dd˙ v


dd˙

0 − → − → − → − → − → → − if v is on the left of u , (ii) det( u , v ) < 0 if v is on the right of u and (iii) → → → → det(− u,− v ) = 0 if − u and − v are collinear. For instance, on Figure 6.7, det(a − − → → m, u ) > 0 and det(b − m, − u ) < 0. Remember as well that the determinant is a multilinear form, i.e.

det(au + bv, cx + dy) = a det(u, cx + dy) + b det(v, cx + dy) = ac det(u, x) +bc det(v, x) + ad det(u, y) +bd det(v, y)

260

Mobile Robotics

/ →0 → P ROOF.– The line D m,− u passing through m and carried by − u separates the plane → − into two half-planes: one that satisfies det(z − m, u ) ≥ 0 and another that verifies → det(z − m, − u ) ≤ 0. This line cuts the / segment 0[ab] if/ a and b−are 0 not in the same → half-plane (see Figure 6.7), i.e. if det a − m, − u · det b − m, → u ≤ 0.

+ →, Figure 6.7. The line D m,− u cuts the segment [ab]

/ →0 In the left figure, the ray E m,− u cuts the segment [ab] , which is not case / the− 0 in → the right figure. The inequality is therefore insufficient for stating that E m, u / 0 / 0 / → 0 cuts − → − → − [ab] . Let us assume that det a − m, u . det b − m, u ≤ 0 (i.e. D m, u cuts / →0 − [ab]). The points of E m,− u satisfy z = m + α→ u , α ≥ 0. The point z belongs → to the [ab] if m 0+ α− u − a and b − a are collinear, i.e. when α satisfies / segment → − det m + α u − a, b − a = 0 (see Figure 6.8). Since the determinant is a multilinear form, we have: → det(m − a, b − a) + α det(− u , b − a) = 0. This yields: α=

det(a − m, b − a) . → det(− u , b − a)

→ If α ≥ 0, then α represents the distance d from m to the next segment − u . If α < 0, then the radius of the telemeter will never reach the segment. The condition α ≥ 0 corresponds to the second inequality that we had to demonstrate.

Identification

261

→ Figure 6.8. If the point m + α− u is on the segment [a, b] then α corresponds to the directional distance

2) In the following, the coordinates of the center m of the robot are denoted by − (x, y) and → u represents a unit vector corresponding to the direction of the laser. For th the k sensor, we have: → − u =

"

/ 0# cos / kπ 4 + θ0 , k ∈ {0, . . . , 7} sin kπ 4 +θ

To have an expression of f (p), we need to calculate the distances returned by the telemeters. By using the theorem shown in question 1), we can deduce the following simulator f (p): Function f (p) 1 (x, y, θ) := p 2 For i := 1 : 8  3 4 5 6 7 8

. " # cos (i−1)π +θ x 4 → −  .  u := ; m := ; *i := ∞ (i−1)π y sin +θ 4 For j := 1 : n det(a −m,bj −aj ) α := det(j→ − j )0 / / u ,bj −a− / 0 0 → If det aj − m, → u · det bj − m, − u ≤ 0 and (α ≥ 0) then *i := min (*i , α) Return (*1 , . . . , *8 )

3) The following program proposes a minimization of the criterion j (p) = -f (p) − y- using the simulated annealing method:

262

Mobile Robotics

2 3 4 5 6 7 8

"

0 7 7 9 9 7 7 4 2 0 5 6 "0 0 2 2 4 4 7 7 5 5 2 2 7 7 9 9 7 7 4 2 0 0 6 6 B := 0 2 2 4 4 7 7 5 5 0 2 3 T y := (6.4, 3.6, 2.3, 2.1, 1.7, 1.6, 3.0, 3.1) T q := (0, 0, 0) ; T = 10 While T > 0.01 p := q + T · randn(I3 ) If j(p) < j(q) then q = p T := 0.99 · T

1 A :=

6 3 5 3

# 5 3# 5 2

This program performs a random search and keeps the best parameter vector in q. The variable T is the temperature that gives the search step. This temperature decreases exponentially with of time. The algorithm generates the solution represented on Figure 6.9.

Figure 6.9. The position of the robot as found from the eight distances given by the laser telemeters

7 Kalman Filter

In Chapters 2 and 3 we looked at tools to control robots in a nonlinear manner. For this purpose, we have assumed that the state vector was completely known. However, this is not the case in practice. This vector must be estimated from sensor measurements. In the case where the only unknown variables are associated with the position of the robot, Chapter 5 gives guidelines to find them. In the more general case, filtering or state observation seeks to reconstruct this state vector as well as possible from all the data measured on the robot throughout time by taking into account the state equations. The aim of this chapter is to show how such reconstruction is performed, within a stochastic context in which the system to observe is assumed to be linear. This is the purpose of the Kalman filter [KAL 60] which will be developed in this chapter. The Kalman filter is used in numerous mobile robotics applications, even though the robots in question are strongly nonlinear. For such applications, the initial conditions are assumed to be relatively well-known in order to allow a reliable linearization. 7.1. Covariance matrices The Kalman filter is mainly based on the concept of covariance matrix, which is important to grasp in order to understand the design and the utilization of the observer. This section recalls the fundamental concepts surrounding covariance matrices. 7.1.1. Definitions and interpretations Let us consider two random vectors x ∈ Rn and y ∈ Rm . The mathematical expectations of x and y are denoted by x ¯ = E (x), y ¯ = E (y). Let us define the

Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

264

Mobile Robotics

@ = x−x @ = y−y variations of x and y by x ¯ and y ¯. The covariance matrix is given by: . / 0 T @·y @ T = E (x − x Γxy = E x ¯) (y − y ¯) .

The covariance matrix for x is defined by:

. / 0 T @·x @T = E (x − x Γx = Γxx = E x ¯) (x − x ¯) .

The one for y is:

. / 0 T @·y @ T = E (y − y Γy = Γyy = E y ¯) (y − y ¯) .

@, y @ are random vectors whereas x Let us note that x, y, x ¯, y ¯, Γx , Γy , Γxy are deterministic. A covariance matrix Γx of a random vector x is always positive definite (we will write Γx > 0), except in the degenerate case. In a computer, a random vector can be represented by a cloud of points associated with realizations. Let us consider the following program: 1 N := 1000;  x2 :=  2 · 1N ×1 + randnN ×1 x1   2 y := 2 ·  ...  + randnN ×1 2

P xN P 3 x ¯ := ¯ = N1 i yi i xi ; y @ := x − x @"= P 4 x ¯# · 1N ×1 ; y y − y¯ · 1P N ×1 " # Γx Γxy x @2i Pi x @i y@i 1 i P := N · 5 Γxy Γy @i y@i @i2 ix iy 1 N

This yields Figure 7.1, which gives us a representation of the random variables x, y (on the left) and of x @, y@ (on the right). The program also gives us the estimations: x ¯ . 1.99, y¯ . 9.983, Γx . 1.003, Γy . 74.03, Γxy . 8.082

where x ¯, y¯, Γx , Γy , Γxy correspond to xbar, ybar, Gx, Gy, Gxy. Two random vectors x and y are linearly independent (or non-correlated or orthogonal) if Γxy = 0. On Figure 7.2, the two point clouds correspond to non-correlated variables. Only the figure on the right corresponds to independent variables.

60

50

50

40

40

30

30

20

20

10

10

0

0

-10

-10 -2

-1

1

0

2

3

4

5

-20 -4

-3

-2

-1

Kalman Filter

265

1

3

0

2

Figure 7.1. Cloud of points that represents a pair of two random variables 15

1.5

10

1.0

5

0.5

0

0.0

-5

-0.5

-10

-1.0

-15 -15

-10

-5

0

5

10

15

-1.5 -1.5

-1.0

-0.5

0.0

0.5

Figure 7.2. Left: dependent but non-correlated variables (x, y); Right: independent variables

The figure on the left was generated by: 1 N := 2000; 2 ρ := 10 · 1N ×1 + randnN ×1 3 θ := 2π    · randnN ×1 ρ1 cos θ1 ρ1 sin θ1     .. .. 4 x :=  ; y :=   . . ρN cos θN ρN sin θN

1.0

1.5

266

Mobile Robotics

And the figure on the right was generated by: 1 N := 2000; 2 x := atan(2 · randnN ×1 ) 3 y := atan(2 · randnN ×1 ) Whiteness. A random vector x is called white if all of its components xi are independent from one another. In such a case, the covariance vector Γx of x is diagonal. 7.1.2. Properties Covariance matrices are symmetric and positive, i.e. all of their eigenvalues are real and positive. The set of all covariance matrices of Rn×n will be denoted by S + (Rn ). Decomposition. Every symmetric matrix Γ can be put into a diagonal form and has an orthonormal eigenvector basis. We may therefore write: Γ = R · D · R−1 where R is a rotation matrix (i.e. RT R = I and det R = 1). The matrix R corresponds to the eigenvectors and D is a diagonal matrix whose elements are the eigenvalues. For the matrices of S + (Rn ), these eigenvalues are positive. Square root. Every matrix Γ of S + (Rn ) has a square root in S + (Rn ). This square 1 root will be denoted by Γ 2 . Following the eigenvalue correspondence theorem, the 1 eigenvalues of Γ 2 are the square roots of those of the eigenvalues of Γ. E XAMPLE.– Consider the following script: 1 A := rand3×3 ; S1 := A · AT 2 [R, D] := eig(S1 ); S2 := R ∗ D ∗ RT 1

3 A2 := S22 ; S3 := A2 · AT2

The matrix D is diagonal and the matrix R is a rotation matrix that contains the eigenvectors of S1 . The three matrices S1 , S2 , S3 are equal. This is not the case for matrices A and A2 since only A2 is symmetric. Note that A2 is a covariance matrix. Order. If Γ1 and Γ2 belong to S + (Rn ), then Γ = α1 Γ1 + α2 Γ2 also belongs to S (Rn ) if α1 ≥ 0 and α2 ≥ 0. This is equivalent to saying that S + (Rn ) is a convex cone of Rn×n . Let us define the order relation: +

Γ1 ≤ Γ2 ⇔ Γ2 − Γ1 ∈ S + (Rn ) .

Kalman Filter

267

It can be easily verified that it is reflexive, antisymmetric and transitive. If Γ1 ≤ Γ2 then the a-level confidence ellipse (see following paragraph) of Γ1 is (in general) included in the one that corresponds to Γ2 . The smaller the covariance matrix (in the sense of this order relation), the more precise it is. 7.1.3. Confidence ellipse A random vector x of Rn can be characterized by the pair (¯ x, Γx ), to which we can associate an ellipse of Rn which encloses the consistent values for x. In practice, for purely graphical reasons, we often only look at two components w = (xi , xj ) of x (a computer screen is in fact two-dimensional). The average w ¯ can be directly th th deduced from x ¯ by extracting the i and j components. The covariance matrix Γw ∈ / 0 S + R2 can also be obtained from Γx ∈ S + (Rn ) by extracting the ith and j th lines and columns. The confidence ellipse associated with w is described by the inequality: T

Ew : (w − w) ¯ Γ−1 ¯ ≤ a2 w (w − w) where a is an arbitrary positive real number. Therefore if w is a Gaussian random vector, this ellipse corresponds to a contour line of the probability density for w. Since − 12 Γ−1 w > 0, it has a square root Γw which is also positive definite. We may therefore write: C D −1 −1 T Ew = w | (w − w) ¯ Γw 2 · Γw 2 (w − w) ¯ ≤ a2 H H C D −1 H H = w | H a1 · Γw 2 (w − w) ¯ H ≤1 C D −1 = w | a1 · Γw 2 (w − w) ¯ ∈ U , where U is the unit disk C D 1 2 = w|w ∈w ¯ + aΓw U 1

2 = w ¯ + aΓw U.

The ellipse Ew can therefore be defined as the image of the unit disk by the affine 1 2 function w(s) = w ¯ + a · Γw s, as illustrated by Figure 7.3. Recall that for a centered, unit Gaussian random vector s, the random variable z = sT s follows a χ2 law. In 2 dimensions, this probability density is given by: / 0 !1 exp − z2 if z ≥ 0 2 πz (z) = 0 otherwise Thus, for a given a > 0, we have: / 0 / 0 def η = prob (||s|| ≤ a) = prob sT s ≤ a2 = prob z ≤ a2 / 0 B a2 1 2 = 0 12 exp − z2 dz = 1 − e− 2 a .

268

Mobile Robotics

Figure 7.3. A confidence ellipse is the image of the unit circle by an affine function

And therefore: : a = −2 ln (1 − η)

This relation allows us to calculate the threshold a that we need to choose in order to have a probability of being in the ellipse of η. But careful, this probabilistic interpretation only makes sense in the Gaussian case. The following function draws Ew for a given probability η. ¯ Γw , η) Function D RAW E LLIPSE (w, " # 1 / 0 : cos 0 cos 0.1 · · · cos 2π 2 ¯ ¯ ¯ w w · · · w 1 w= + −2 ln (1 − η) · Γw · sin 0 sin 0.1 · · · sin 2π 2 plot(w) 7.1.4. Generating Gaussian random vectors If we generate n centered Gaussian random numbers, we obtain the realization of a random vector whose center is x ¯ = 0 and whose covariance matrix Γx is the identity matrix. In this section we will show, given a centered Gaussian random number generator allowing us to realize x, how we can obtain a Gaussian random vector y of dimension n with an expectation and covariance matrix Γy . The main principle of this generation is based on the following theorem. T HEOREM.– If x, α and y are three random vectors connected by the relation y = Ax + α + b (where A and b are deterministic), and assuming that x, α are independent and that α is centered, we have: y ¯ = A¯ x+b Γy = A · Γx · A T + Γα

[7.1]

Kalman Filter

269

P ROOF.– We have: y ¯ = E (Ax + α + b) = AE (x) + E (α) + b = A¯ x+b Moreover:

. T Γy = E (y − y ¯) (y − y ¯) . T = E (Ax + α + b − A¯ x − b) (Ax + α + b − A¯ x − b) . T = E (A@ x + α) · (A@ x + α) / 0 / 0 / 0 / 0 @·x @ T · AT + A · E x @ · αT + E α · x @ T · AT + E α · α T =A·E x 2 34 5 2 34 5 2 34 5 2 34 5 =0

=Γx

=0

=Γα

T

= A · Γx · A + Γα

which concludes the proof. ! Thus, if x is a centered, unit Gaussian white random noise (i.e. x ¯ = 0 and Γx = I), 1 2 the random vector y = Γy x+ y ¯ will have an expectation of y ¯ and a covariance matrix equal to Γy (see Figure 7.4). To generate a Gaussian random vector with covariance matrix Γy and expectation y ¯, we will use this property. The right side of Figure 7.4 was thus obtained by the script: "

# " # 3 1 2 1 N := 1000; Γy := ; y ¯= ; X := randn2×n 1:3 3 2 Y := (¯ y, y ¯, . . . , y ¯ ) + Γy · X 3 plot(Y) 7.2. Unbiased orthogonal estimator Let us consider two random vectors x ∈ Rn and y ∈ Rm . The vector y corresponds to the measurement vector which is a random vector. Later, when the measurements will have been made, it will become deterministic. The random vector x is the vector we need to estimate. An estimator is a function φ (y) that gives us an estimation of x given the knowledge of the measurement y. Figure 7.5 shows a nonlinear estimator corresponding to E (x|y). However, obtaining an analytic expression for such an estimator is generally not a simple task and it is preferable to limit ourselves to linear estimators. A linear estimator is a linear function of Rm → Rn of the form: x ˆ = Ky + b

[7.2]

270

Mobile Robotics

where K ∈ Rn×m and b ∈ Rn . In this section, we will propose a method capable of finding a good K and a good b from the sole knowledge of the first-order moments x ¯, y ¯ and the second-order moments Γx , Γx , Γxy . The estimation error is: ε=x ˆ−x

Figure 7.4. The Gaussian random vector y : (¯ y, Γy ) is the image by an affine application of a unit Gaussian white random vector x

Figure 7.5. Nonlinear estimator E(x|y)

/ T0 The estimator is said to be unbiased if E (ε) = 0. It is orthogonal if E ε@ y = 0. This naming comes from the fact that the space of random variables of R can be / / 00 equipped with a scalar product defined by *a, b+ = E (a − a ¯) b − ¯b and that if

Kalman Filter

271

this scalar product is zero, the two random variables a and b are called orthogonal. @ are vectors), we In the vector case (which is that of our paragraph since ε and y say that the two random vectors a and b are orthogonal if their components are,.i.e. / / 00 / 0 ¯ T = E (ai − a ¯i ) bj − ¯bj = 0 for all (i, j), or equivalently E (a − ¯ a) b − b 0. Figure 7.6 represents the contour lines of a probability law for the pair (x, y). The line illustrates a linear estimator. Let us randomly pick a pair (x, y) while respecting its probability law. It is clear that the probability to be above the line is high, i.e. the probability to have x ˆ < x is high, or even that E (ε) < 0. The estimator is thus biased. Figure 7.7 represents four different linear estimators. For estimator (a), E(ε) < 0 and for estimator (c), E(ε) > 0. For estimators (b) and (d), E(ε) = 0 and therefore the two estimators are unbiased. However, it is evident that estimator (b) is better. What differentiates these two is orthogonality. For (d), we have E (ε@ y ) < 0 (if y@ > 0, ε tends to be negative whereas if y@ < 0, ε tends to be positive).

Figure 7.6. Biased linear estimator

T HEOREM.– Consider two random vectors x and y. A unique unbiased orthogonal estimator exists. It is given by: x ˆ=x ¯ + K · (y − y ¯)

[7.3]

K = Γxy Γ−1 y

[7.4]

where:

is referred to as the Kalman gain.

272

Mobile Robotics

Figure 7.7. Among these four linear estimators, estimator (b), which is unbiased and orthogonal, seems to be the best

E XAMPLE.– Let us consider once again the example in section 7.1.1. We obtain: x ˆ=x ¯ + Γxy Γ−1 ¯) = 2 + 0.1 · (y − 10) . y · (y − y The corresponding estimator is illustrated on Figure 7.8.

Figure 7.8. Unbiased orthogonal linear estimator

Kalman Filter

273

P ROOF OF THE THEOREM.– We have: [7.2]

E (ε) = E (ˆ x − x) = E (Ky + b − x) = KE (y) + b − E (x) = K¯ y+b−x ¯. The estimator is unbiased if E (ε) = 0, i.e.: [7.5]

b=x ¯ − K¯ y, which gives us [7.3]. In this case: [7.3]

@. ε=x ˆ−x = x ¯ + K · (y − y ¯) − x = K@ y−x

[7.6]

The estimator is orthogonal if: / 0 [7.6] @T = 0 ⇔ E ε·y ⇔ ⇔ ⇔

/ 0 @) · y @ T0 = 0 E /(K@ y−x @T − x @y @T = 0 E K@ yy KΓy − Γxy = 0 K = Γxy · Γ−1 y

which concludes the proof. ! T HEOREM.– The covariance matrix of the error associated with the unbiased orthogonal linear estimator is: Γε = Γx − K · Γyx .

[7.7]

P ROOF.– The covariance matrix of ε in the unbiased case is written as: . / 0 [7.6] @) · (K@ @ )T Γε = E ε · εT = E (K@ y−x y−x / / T T 00 @) · y @ K −x @T = E (K@ y−x / 0 @ T KT − x @y @ T KT − K@ @T + x @x @T . = E K@ yy yx Using the linearity of the expectation operator, we get Γε = (KΓy − Γxy )KT − KΓyx + Γx . ! 34 5 2

[7.8]

[7.4]

=0

We will now present a theorem that shows that the unbiased orthogonal linear estimator is the best among all unbiased estimators. In order to understand this concept of best, we need to recall the inequalities on the covariance matrices (see section 7.1), which tells us that Γ1 ≤ Γ2 if and only if ∆ = Γ2 − Γ1 is a covariance matrix.

274

Mobile Robotics

T HEOREM.– No unbiased linear estimator exists allowing to obtain a smaller covariance matrix on the error Γε than the one given by the orthogonal estimator. P ROOF.– Every possible matrix K for our unbiased linear estimator is written in the form K = K0 + ∆ with K0 = Γxy Γ−1 y and ∆ being an arbitrary matrix. Following [7.8], the covariance matrix for the error is: T

Γε = ((K0 + ∆) Γy − Γxy ) (K0 + ∆) − (K0 + ∆) Γyx + Γx = (K0 + ∆) (Γy KT0 + Γy ∆T ) − (Γxy KT0 + Γxy ∆T ) 2 34 5 2 34 5 =K0 Γyx

=Γyx

− (K0 Γyx + ∆Γyx ) + Γx = K0 Γyx + ∆Γyx + K0 Γy ∆T + ∆Γy ∆T − K0 Γyx − Γxy ∆T 2 34 5 =Γxy

−K0 Γyx − ∆Γyx + Γx = −K0 Γyx + ∆Γy ∆T + Γx .

Since ∆Γy ∆T is always positive symmetric, the covariance matrix Γε is minimal for ∆ = 0, i.e. for K = Γxy Γ−1 y , which corresponds to the orthogonal unbiased estimator. ! 7.3. Application to linear estimation Let us assume that x and y are related by the relation y = Cx + β where β is a centered random vector non-correlated with x. The covariance matrices of x and β are denoted by Γx and Γβ . Let us use the results obtained in the previous section in order to find the best unbiased linear estimator for x (refer to [WAL 14] for more details on linear estimation). We have: y ¯ Γy Γxy

¯ = C¯ = C¯ x+β x [7.1]

= CΓx CT + Γβ " .T # . / 0 @ @T @·y @T = E x @ · C@ @·x @ T CT + x @·β = E x x+β =E x . / 0 @ T = Γx C T . @·x @ T CT + E x @·β = E x 2 34 5 =0

[7.9]

Kalman Filter

275

Consequently, the best unbiased estimator for x and covariance matrix of the error can be obtained from Γx , Γβ , C, x ¯ by using the following formulas: (i) x ˆ

[7.3]

=

[7.7]

(ii) Γε = @ (iii) y

[7.9]

=

[7.9]

(iv) Γy = (v) K

[7.4,7.9]

=

x ¯ + K@ y

(estimation)

Γx − KCΓx (covariance of the error) y − C¯ x

(innovation)

Γx CT Γ−1 y

(Kalman gain)

[7.10]

CΓx CT + Γβ (covariance of the innovation)

N OTE.– Figure 7.5 illustrates a situation in which it could be advantageous not to use a linear estimator. Here, the chosen estimator corresponds to x ˆ = E (x|y). In the particular case where the pair (x, y) is Gaussian, the estimator x ˆ = E (x|y) corresponds to the unbiased orthogonal estimator. In this case, we have, following [7.10]: −1 E (x|y) = x ¯+ ¯) . - Γxy Γy (y − y / 0 T E ε · εT |y = E (ˆ x − x) (ˆ x − x) |y = Γx − Γxy Γ−1 y Γyx .

7.4. Kalman filter This paragraph presents the Kalman filter (refer to [DEL 93] for more information). Let us consider the system described by the following state equations: !

xk+1 = Ak xk + uk + αk yk = Ck xk + β k

where αk and β k are random, independent Gaussian noises white in time. By white in time we mean that the vectors αk1 and αk2 (or β k1 and β k2 ) are independent of one another if k1 &= k2 . The Kalman filter alternates between two phases: correction and prediction. To understand the mechanism of the filter, let us position ourselves at time k and assume that we have already processed the measurements y0 , y1 , . . . , yk−1 . At this stage, the state vector is a random vector that we will denote by xk|k−1 (since we are at time k and the measurements have been processed until k − 1). This random vector is represented by an estimation denoted by x ˆk|k−1 and a covariance matrix Γk|k−1 . Correction. Let us take the measurement yk . The random vector representing the state is now xk|k , which is different than xk|k−1 since xk|k has knowledge of the

276

Mobile Robotics

measurement y. The expectation x ˆk|k and the covariance matrix Γk|k associated to xk|k are given by equations [7.10]. We therefore have: (i) (ii) (iii) (iv) (v)

x ˆk|k Γk|k @k y Sk Kk

= = = = =

@k x ˆk|k−1 + Kk · y Γk|k−1 − Kk · Ck Γk|k−1 yk − C k x ˆk|k−1 Ck Γk|k−1 CTk + Γβk Γk|k−1 CTk S−1 k

(corrected estimation) (corrected covariance) (innovation) (covariance of the innovation) (Kalman gain) [7.11]

Prediction. Given the measurements y0 , y1 , . . . , yk , the random vector representing the state is now xk+1|k . Let us calculate its expectation x ˆk+1|k and its covariance matrix Γk+1|k . Since xk+1 = Ak xk + uk + αk , we have, following equation [7.1]: x ˆk+1|k = Ak x ˆk|k + uk

[7.12]

Γk+1|k = Ak · Γk|k · ATk + Γαk .

[7.13]

and

Kalman filter. The complete Kalman filter is given by the following equations: x ˆk+1|k

[7.12]

=

[7.13]

Γk+1|k = x ˆk|k Γk|k @k y

Sk Kk

[7.10,i]

=

[7.10,ii]

=

[7.10,iii]

=

[7.10,iv]

(predicted estimation)

Ak x ˆk|k + uk Ak · Γk|k ·

ATk

+ Γαk (predicted covariance) (corrected estimation)

@k x ˆk|k−1 +Kk · y

(I − Kk Ck ) Γk|k−1 yk − C k x ˆk|k−1

= Ck Γk|k−1 CTk + [7.10,v] = Γk|k−1 CTk S−1 k .

Γβ k

(corrected covariance) (innovation) (covariance of the innovation) (Kalman gain)

Figure 7.9 illustrates the fact that the Kalman filter stores the vector x ˆk+1|k and the matrix Γk+1|k . Its inputs are yk , uk , Ak , Ck , Γαk and Γβk . The quantities x ˆk|k , @k , Sk , Kk are auxiliary variables. The delay is activated by a clock and when Γk|k , y activated, k is incremented by 1.

The function K ALMAN below implements the Kalman filter and returns x ˆk+1|k , Γk+1|k . In this program, we have the following correspondences: x ˆpred ↔ x ˆk|k−1 , Γpred ↔ Γk|k−1 , x ˆup ↔ x ˆk|k , Γup ↔ Γk|k (the term up refers to update, i.e. correction).

Kalman Filter

277

Figure 7.9. The Kalman filter is composed of a corrector followed by a predictor

1 2 3 4 5 6 7 8

Function K ALMAN (ˆ xpred , Γpred , u, y, Γα , Γβ , A, C) pred T S := C · Γ · C + Γβ K := Γpred · CT · S−1 @ := y − C · x y ˆpred up pred @ x ˆ := x ˆ +K · y Γup := (I − K · C) Γpred ˆ pred := A · x x ˆup + u pred T Γ := · Γup · A / A 0 + Γα pred pred ˆ ,Γ Return x

Positivity lost. Due to numerical problems, the covariance of the innovation Sk can sometimes lose its positivity. If such a problem arises, it is preferable to replace the corrected covariance equation with: < T Γk|k := (I − Kk Ck ) Γk|k−1 ΓTk|k−1 (I − Kk Ck )

which will always be positive definite, even when the matrix Γk|k−1 is not. The Kalman filter equations will then be more stable in the sense that a slight error on the positive character of the covariance matrices is removed in the next iteration. Predictor. When no measurement is available, the Kalman filter operates in predictor mode. In order to be able to use the Kalman function, y, Γβ , C have to become empty quantities. However, they have to have the correct dimensions in order to perform matrix operations.

278

Mobile Robotics

Initialization. Most of the time, we have no idea of the initial state x0 . In this case, we generally set 

1 ε2

0  x ˆ0 = (0, 0, . . . , 0) and Γx (0) =  0

0 0 1 ε2



  , .. . 0  0 ε12

where ε in a small positive number (for instance 0.001). This assumption amounts more or less to say that x0 is inside a sphere that is centered in at zero and that has radius 1ε . Stationary case. For time-independent systems, Γk+1|k converges to a matrix Γ. For large k, Γ = Γk+1|k = Γk|k−1 . Thus, Γk+1|k = A · Γk|k · AT + Γα T = A · (I ·A / −K / k C) Γk|k−1 0 0+ Γα T −1 = A · -I − -Γk|k−1 C S C Γk|k−1 · AT + . Γα . / 0−1 T T = A · I − Γk|k−1 C CΓk|k−1 C + Γβ C Γk|k−1 · AT + Γα Therefore, for large k,

. / 0−1 Γ = A · Γ − Γ · CT · C · Γ · CT + Γ β · C · Γ · AT + Γ α ,

which is a Ricatti equation in Γ. It can be solved by the sequence

. / 0−1 Γ(k + 1) = A · Γ(k) − Γ(k) · CT · C · Γ(k) · CT + Γβ · C · Γ(k) ·AT + Γα

up to the equilibrium and starting with large Γ(0) (e.g. 1010 · I). This computation can be done before the implementation of the filter, which allows us to avoid unnecessary real-time computation of Γk+1|k . The corresponding stationary Kalman filter is thus / 0 x ˆk+1|k = Aˆ xk|k−1 +AK · yk − Cˆ xk|k−1 + uk

/ 0−1 with K = ΓCT CΓCT + Γ .

Kalman Filter

279

7.5. Kalman–Bucy We now consider the linear continuous-time system described by ! x˙ t = At xt + ut + αt y t = C t xt + β t where ut and yt are known for all t. The noise αt and β t are assumed to be white and Gaussian. For a better understanding, the order of the quantities that will be used with respect to dt (assumed to be infinitely small) are recalled below. Size Infinitely small Medium Infinitely large

Order Random Deterministic O (dt) αk Kt ,uk O/(1)0 xt , xk Γ α , Γ β , K b , u t , A t , Ct 1 O dt αt , β t , β k , x˙ t , yt Γαt , Γβ t

The covariance matrices Γαt ,Γβt for αt and β t are infinite (O 1 1 Γαt = dt · Γα and Γβt = dt · Γβ .

/10 dt ), and we write

N OTE .– x˙ t , yt are considered as infinite because the noises αt , β t are also infinite. If all had been finite, the state could have been found in an infinitely small time. To be able to deal with these infinitely large quantities properly, distribution theory is needed. Here, we will not use these difficult notions and will consider classical notions from signal processing. Note that this kind of simplification is usually made in physics, for instance when we assimilate the Dirac distribution with the Dirac function δ0 (t). Since we have B t+dt xt+dt = xt + t x˙ dτ B t+dt τ = xt + t (Aτ xτ + uτ + ατ ) dτ B t+dt xt + dt · (At xt + ut ) + t ατ dτ,

a discretization with a sampling time dt is  = t+dt    x = (I + dt · A ) x +dt · u + ατ dτ t+dt t t t   34 52345 2 34 5  2 34 5 2 t 2 34 5 xk uk xk+1 Ak αk   yt = Ct x t + β t .   2345 2345   2345 yk

xk

βk

Note that, as explained in exercise 7.6, the covariance for the state noise αk increases linearly with dt. More precisely, Γαk = dt · Γα .

280

Mobile Robotics

This means that when dt is infinitely small, αk is infinitely small and αt is infinitely large. The measurement yk represents a fusion of all measurement yt with t ∈ [t, t + dt]. As for β t , its covariance matrix is Γβ k =

1 · Γβ . dt

We now have a linear discrete-time system, and we can apply the classical Kalman filter given in section 7.4. We have the following correspondences: Ak x ˆk+1|k Γk+1|k Γαk

→ (I + dtAt ) → x ˆt+dt → Γt+dt → dt · Γα

uk x ˆk|k−1 Γk|k−1 Γβ k

→ dt · ut → x ˆt → Γt 1 → dt · Γβ

αk → x ˆk|k → Γk|k →

B t+dt t

ατ dτ ˚ xt ˚ Γt

The Kalman filter becomes  x ˆt+dt = (I + dtAt )˚ xt + dt · ut     Γt+dt = (I + dtAt ) · ˚ Γt · (I + dtAt )T + dtΓα     @ xt = x ˆt +Kt · yt ˚ ˚ Γt = (I − Kt Ct ) · Γt    @t y = yt − Ct x ˆt   1   St = Ct Γt CTt + dt · Γβ   T −1 Kt = Γ t C t St

1 Taking into account the fact that Ct Γt CTt is small (line (vi)) compared to dt · Γβ 1 (which is infinitely large) and we can simplify it, i.e. St = dt · Γβ (t), We conclude that St is huge (= O(1/dt)) and thus that S−1 at line (viii) is small (= O(dt)). The t Kalman filter becomes  ˆt+dt = (I + dtAt ) (ˆ xt +Kt · (yt − Ct x ˆt )) + dt · ut x T Γt+dt = (I + dtAt ) · (I − Kt Ct ) Γt · (I + dtAt ) + dtΓα  T −1 Kt = dtΓt Ct Γβ .

Since Kt is small (= O(dt)), it is more convenient to introduce the Kalman-Bucy gain given by Kb = Γt CTt Γ−1 β which is O(1), i.e. neither infinitely small nor infinitely large. We have  x ˆt+dt = (I + dtAt ) (ˆ xt + dtKb · (yt − Ct x ˆt )) + dt · ut    2 34 5    =(I+dtAt )ˆ xt +dtKb ·(yt −Ct x ˆt )+O(dt)2  T Γt+dt = (I + dtAt ) · (I − dtKb Ct ) Γt · (I + dtAt ) + dtΓα 2 34 5    2  =Γt +dt(At Γt −Kb Ct Γt +Γt AT t )+O(dt)   K = Γt CTt Γ−1 b β

Kalman Filter

281

Or equivalently  x ˆt+dt −ˆ xt  = At x ˆ + Kb · (yt − Ct x ˆ t ) + ut  dt / t 0 Γt+dt −Γt T = A Γ − K C Γ + Γ t t b t t t At + Γ α dt  K = Γt CTt Γ−1 b β .

/ 0T T Now, Ct Γt = Γt CTt = (Kb Γβ ) = Γβ KTb . Therefore, we get the Kalman– Bucy filter:   

d ˆt dt x d Γ dt t

Kb

= At x ˆt + Kb (yt − Ct x ˆ t ) + ut = At Γt + Γt · ATt − Kb Γβ KTb + Γα = Γt CTt Γ−1 β

The advantage of the latter formulation compared to the former is that the product Kb Γβ KTb is more stable than Kb Ct Γt , since it preserves the symmetry of the resulting matrix. This Kalman–Bucy filter corresponds to a continuous version of the Kalman filter. In its formulation, all/ quantities are O (1), and thus no more infinitely 0 1 small (O(dt)) or infinitely large (O dt ) quantities appear. N OTE.– The Kalman–Bucy filter allows us to understand some effects that occur when the discrete-time Kalman filter is used for continuous-time systems, such as a robot described by ! x˙ = fc (x, u) y = g(x)

To perform a simulation, we discretize it, for instance using a Euler integration. We get ! xk+1 = f (xk , uk ) + αk yk = g(xk ) + β k where f (xk , uk ) = xk + dt · fc (xk , uk ) and where αk , β k is the noise. Now, a good tuning for the covariances matrices Γα , Γβ at a given sampling time dt, should remain good if we change dt. The Kalman–Bucy filter tells us that the covariance matrices should depend on dt as follows: Γα = dt · Γ0α 1 Γβ = dt · Γ0β In such a case, the behavior of the simulated system will not be affected by a change of dt. As a consequence, if we change dt → 2 · dt (for instance √ to make the simulation go twice as fast), we need to multiply the noises αk , β k , by 2, √12 , respectively.

282

Mobile Robotics

7.6. Extended Kalman filter As we have seen in previous chapters, a robot is described by continuous-state equations of the form !

x˙ = fc (x, u) y = g(x)

which are nonlinear. A Kalman filter can still be used to estimate the state vector x, but we need to discretize the system and linearize it. A possible discretization can be performed with a Euler method. We get !

xk+1 = xk + dt · fc (xk , uk ) = f (xk , uk ) yk = g(xk )

ˆ k of the state To linearize the system, we assume that we have an estimation x vector. Therefore, A

k ,uk ) ˆk) xk+1 . f (ˆ xk , uk ) + ∂f (ˆx∂x · (xk − x xk ) ˆ yk . g(ˆ xk ) + dg(ˆ (x − x ) k k dx

If we set Ak =

∂f (ˆ x k , uk ) dg(ˆ xk ) and Ck = , ∂x dx

we get    

xk+1

ˆk) . Ak · xk + (f (ˆ xk , u k ) − Ak · x 2 34 5 vk

ˆ k ) . Ck · x k . (yk − g(ˆ xk ) + C k · x   2 34 5 zk

This approximation can be written as !

xk+1 = Ak xk + vk + αk zk = Ck xk + β k

The noises αk and β k are non-Gaussian and are not white, since they include some linearization errors. A classical Kalman filter can be used (with vk and zk , which are known, taking the role of uk and yk ), but its behavior is not reliable anymore. If we are lucky, the results will not be so bad, even if they are too optimistic in general. But very often, we are not lucky and the filter provides wrong results. Moreover, it is

Kalman Filter

283

difficult to quantify the linearization errors in order to deduce covariance matrices for the noises. ˆ k corresponds to x In our context, x ˆk|k−1 , the estimation we have for the state xk , taking into account all the past measurements. But it can also correspond to x ˆk|k , when available. Therefore, the linearized Kalman filter, called the extended Kalman filter, is ∂f (ˆ x

,u )

k k|k Ak = (evolution matrix) ∂x dg(ˆ xk|k−1 ) Ck = dx / 0 (observation matrix) x ˆk+1|k = Ak x ˆk|k + f (ˆ xk|k , uk ) − Ak · x ˆk|k 2 34 5

vk

Γk+1|k x ˆk|k Γk|k @ zk

= = = =

= f (ˆ xk|k , uk ) Ak · Γk|k · ATk + Γαk x ˆk|k−1 +Kk · @ zk (I − K C ) Γ k k k|k−1 / 0 yk − g(ˆ xk|k−1 ) + Ck · x ˆk|k−1 2 34 5 zk

−Ck x ˆk|k−1 = yk − g(ˆ xk|k−1 ) Sk = Ck Γk|k−1 CTk + Γβk Kk = Γk|k−1 CTk S−1 k

(predicted estimation) (predicted covariance) (corrected estimation) (corrected covariance)

(innovation) (covariance of the innovation) (Kalman gain)

The extended Kalman filter takes x ˆk|k−1 , Γk|k−1 , yk , uk as inputs and returns x ˆk+1|k and Γk+1|k . 7.7. Exercises E XERCISE 7.1.– Gaussian distribution The probability distribution of a random Gaussian vector x is fully characterized by its expectation x ¯ and its covariance matrix Γx . More precisely, it is given by: " # 1 1 T −1 πx (x) = : · exp − (x − x ¯ ) · Γ · (x − x ¯ ) . x n 2 (2π) det(Γx ) 1) Draw the graph and the contour lines of πx with: " # " # 1 1 0 x ¯= and Γx = . 2 0 1

2) We define the random vector: #" # " # " cos π6 − sin π6 1 0 2 y= x+ . sin π6 cos π6 0 3 −5

284

Mobile Robotics

Draw the graph and the contour lines of πy . Discuss. E XERCISE 7.2.– Confidence ellipses Let us generate six covariance matrices as follows: " # " # 1 0 cos π4 − sin π4 A1 = A2 = 0 3 sin π4 cos π4 Γ1 = I2×2 Γ2 = 3Γ1 Γ4 = A2 · Γ3 · AT2 Γ5 = Γ4 + Γ3

Γ3 = A1 · Γ2 · AT1 + Γ1 Γ6 = A2 · Γ5 · AT2

Here, A2 corresponds to a rotation matrix of angle π4 . Then, we draw the six confidence ellipses at 90 % associated with these matrices by centering them around 0. We thus obtain Figure 7.10. 1) Associate each covariance matrix with its confidence ellipse on the figure. 2) Verify the result by regenerating these ellipses.

Figure 7.10. Confidence ellipses associated with the six covariance matrices

E XERCISE 7.3.– Confidence ellipse: prediction 1) Generate a cloud of N = 1 000 points representing a random Gaussian vector centered at R2 whose covariance matrix is the identity matrix. Deduce from the latter a cloud of points for the random vector x such that: " # " # 1 4 3 x ¯= and Γx = 2 3 3 Use a two-line, n-column matrix X to store the cloud.

Kalman Filter

285

2) Assuming we know x ¯ and Γx draw the confidence ellipses for the probabilities η ∈ {0.9, 0.99, 0.999}. Show graphically the consistency with the cloud X. 3) Find an estimation of x ¯ and Γx from the cloud X.

4) This distribution represents the knowledge we have of the initial conditions of a system (a robot, for instance) described by state equations of the form: " # " # 0 1 2 x˙ = x+ u −1 0 3 where the input u (t) = sin t is known. Write a program that illustrates the evolution of this particle cloud with time. Use a sampling period of dt = 0.01. For the discretization, we take an Euler integration: " # # "" # 2 0 1 u(k) + α(k) x(k + 1) = x(k) + dt x+ 3 −1 0 where α(k) : N (0, dt · I2 ) is a white Gaussian noise. Draw the cloud for t ∈ {0, 1, 2, 3, 4, 5}.

5) Represent this evolution using only a Kalman prediction. Compare the resulting confidence ellipses with the cloud computed at Question 4. E XERCISE 7.4.– Confidence ellipse: correction

1) As in the previous exercise, generate a Gaussian point cloud of n = 1 000 points associated with the random vector x with: " # " # 1 3 1 x ¯= and Γx = . 2 1 3 Use a two-line, n-column matrix to store the clouds. 2) Find an unbiased and orthogonal linear estimator which allows to find x1 from x2 . Draw this estimator. 3) Same question as above, but one that allows to find x2 from x1 . Draw the estimator and discuss the difference with the previous question. E XERCISE 7.5.– Covariance matrix propagation Consider three centered random vectors a, b, c with covariance matrices equal to the identity matrix. These three vectors are independent of each other. Let x, y be two random vectors defined as follows: x = A·a−b y = C·x+c where A, C are matrices that are known.

286

Mobile Robotics

1) Give the expression of the mathematical expectations x ¯, y ¯ and of the covariance matrices Γx , Γy of these two vectors, in function of A and C. 2) We form the vector v = (x , y). Calculate the mathematical expectation v ¯ and the covariance matrix Γv for v. 3) Deduce from the previous question the covariance matrix of the random vector z = y − x. We assume of course that x and y are of same dimension. 4) We measure y, which means that now the random vector y becomes deterministic and is well-known. Give an estimation x ˆ for x using an unbiased and orthogonal linear estimator. E XERCISE 7.6.– Brownian noise We consider a random stationary, discretized, white and centered random signal. This signal is denoted by x (tk ) with k ∈ N. More precisely, for every tk = kδ the random variables x (tk ) with variance σx2 are independent of one other. A Brownian noise is defined as the integral of a white noise. In our case, we form the Brownian noise as follows: y (tk ) = δ ·

k 1

x (tk ) .

i=0

1) Calculate, in function of time, the variance σy2 (tk ) of the signal y (tk ). How does the standard deviation σy (tk ) evolve in function of δ and in function of tk ? Discuss. Validate the result with a simulation. 2) We now tend δ towards 0. What standard deviation σx do we have to choose in function of δ in order for the variances σy2 (tk ) to remain unchanged? Illustrate this program that generates Brownian noises y (t) that are insensitive to sampling period changes. E XERCISE 7.7.– Solving three equations using a linear estimator The linear estimator can be used to solve problems that can be translated as linear equations. Let us consider as an illustration the system:   2x1 + 3x2 = 8 3x1 + 2x2 = 7  x1 − x2 = 0

Since we have more equations than unknowns, the linear estimator must find some sort of compromise between all of these equations. Let us assume that the errors εi

Kalman Filter

287

over the ith equation are centered and with variances: σ12 = 1, σ22 = 4 and σ32 = 4. Solve the system by using a linear estimator and find the associated covariance matrix. E XERCISE 7.8.– Estimating the parameters of an electric motor using a linear estimator Let us consider a DC motor whose parameters have been estimated with a least squares method (see exercise 6.3). Recall that in that example, the angular speed Ω of a DC motor verifies the relation: Ω = x 1 U + x 2 Tr where U is the input voltage, Tr is the resistive torque and x = (x1 , x2 ) is the vector of parameters that we need to estimate. The following table recalls the measurements made on the motor for various experimental conditions: U (V) 4 10 10 13 15 Tr (Nm) 0 1 5 5 3 Ω(rad/ sec) 5 10 8 14 17 We assume that the variance of the measurement error is equal to 9 and does not depend on the experimental conditions. Moreover, we assume that we know a priori that x1 . 1 and x2 . −1 with a variance of 4. Estimate the parameters of the motor and find the associated covariance matrix. E XERCISE 7.9.– Trochoid 1) A point mass (placed on a wheel) is moving following a trochoid of the form: !

x (t) = p1 t − p2 sin t y (t) = p1 − p2 cos t

where x corresponds to the abscissa and y to the altitude of the mass. We measure y for various instants t: t(sec) 1 2 3 7 y(m) 0.38 3.25 4.97 −0.26 The measurement errors have a standard deviation of 10 cm. By using an unbiased orthogonal filter, calculate an estimation for p1 and p2 . 2) Draw the estimated path of the mass.

288

Mobile Robotics

E XERCISE 7.10.– Solving three equations using a Kalman filter Let us consider once again the linear equations of exercise 7.7:   2x1 + 3x2 = 8 + β1 3x1 + 2x2 = 7 + β2  x 1 − x 2 = 0 + β3

where β1 , β2 , β3 are three independent, centered random variables with respective variances 1, 4, 4. 1) Solve this system by calling the Kalman filter three times. Give an estimation of the solution and find the covariance matrix of the error. 2) Draw the confidence ellipses associated with each call. 3) Compare these with the results obtained for exercise 7.7. E XERCISE 7.11.– Three-step Kalman filter Let us consider the discrete-time system: !

xk+1 = Ak xk + uk + αk yk = C k x k + βk

with k ∈ {0, 1, 2}. The values for the quantities Ak , Ck , uk , yk are given by: k " A k # " u k # Ck / 0 0.5 0 8 11 0 0 1 16 " # " # / 0 1 −1 −6 11 1 " 1 1 # "−18# / 0 1 −1 32 11 2 1 1 −8

yk 7 30 −6

Let us assume that the signals αk and βk are white Gaussian signals with a unitary covariance matrix. We have " # 1 0 Γα = and Γβ = 1. 0 1 The initial state vector is unknown and is represented by an estimation x ˆ0|−1 and a covariance matrix Γ0|−1 . We will take: x ˆ0|−1

" # " # 0 100 0 = , Γ0|−1 = . 0 0 100

Kalman Filter

289

Draw the confidence ellipses with center x ˆk|k and covariance matrix Γk|k obtained by the Kalman filter. E XERCISE 7.12.– Estimating the parameters of an electric motor Let us consider once more the DC motor with angular speed Ω (see exercises 6.3 to 7.8). We have: Ω = x 1 U + x 2 Tr where U is the input voltage, Tr is the resistive torque and x = (x1 , x2 ) is the vector of parameters to estimate. The following table presents the measurements obtained for various experimental conditions: k U (V) Tr (Nm) Ω(rad/ sec)

0 4 0 5

1 10 1 10

2 10 5 11

3 13 5 14

4 15 3 17

We still assume that the variance of the measurement error is equal to 9 and that x1 . 1 and x2 . −1 with a variance of 4. Using the Kalman filter, calculate an estimation of the parameters x1 , x2 and give the associated covariance matrix. E XERCISE 7.13.– Localization from wall distance measurements Consider a punctual robot positioned at x = (x1 , x2 ). This robot measures its distance to the three walls, as shown in Figure 7.11. The ith wall corresponds to a line defined by two points a (i) and b (i). The distance to the ith wall is: d (i) = det (u (i) , x − a (i)) + βi b(i)−a(i) with u (i) = &b(i)−a(i)& . Each distance is measured with a centered error βi with variance 1 and all the errors are independent of one other. Before taking any measurements, the robot assumes it is in position x ¯ = (1, 2) with the associated covariance matrix given by 100 · I where I is the identity matrix.

1) Give, in function of the a (i) , b (i) , d (i), an estimation of the robot’s position, as well as the covariance matrix for the error. For this you can use the expression of the unbiased orthogonal linear estimator or equivalently the expression of the Kalman filter in correction mode.

290

Mobile Robotics

Figure 7.11. The robot must locate itself by measuring its distance to the three walls

2) The coordinates of the points as well as the distances are given by: i

"1# " 2 # " 3 # 2 15 3 a (i) 1 5 12 " # " # " # 15 3 2 b (i) 5 12 1 d(i) 2 5 4 Write a program that gives us the required estimation. E XERCISE 7.14.– Temperature estimation The temperature in a room has to verify (after temporal discretization) the state equation: !

xk+1 = xk + αk yk = xk + βk

We assume that the state noise αk and the measurement noise βk are independent and Gaussian with covariance Γα = 4 and Γβ = 3. 1) Give the expression of the Kalman filter that allows to estimate the temperature xk from the measurement yk . From this deduce an expression of x ˆk+1|k and Γk+1|k in function of x ˆk|k−1 , Γk|k−1 ,yk .

Kalman Filter

291

2) For large enough k, we may assume that Γk+1|k = Γk|k−1 = Γ∞ . We then obtain the so-called asymptotic Kalman filter. Give the expression of the asymptotic Kalman filter. How would you characterize the precision of this filter? 3) Going back to the non-asymptotic case, but now assuming that Γαk = 0, what is the value of Γ∞ ? Discuss. E XERCISE 7.15.– Blind walker We consider a blind walker moving on a horizontal line. Its movement is described by the discretized state equation: !

x1 (k + 1) = x1 (k) + x2 (k) · u (k) x2 (k + 1) = x2 (k) + α2 (k)

where x1 (k) is the position of the walker, x2 (k) is the length of a step (referred to as scale factor) and u (k) is the number of steps per time unit. We measure the quantity u (k). Thus, at each unit of time, the walker moves a distance of x2 (k) u (k). At the initial moment, we know that x1 is zero and that x2 is close to 1. x2 (0) will be represented by a Gaussian distribution whose mean is equal to 1 and whose standard deviation is 0.02. The scale factor x2 evolves slowly by means of α2 (k) that we will assume to be centered, white and of standard deviation 0.01. 1) We apply an input u (k) = 1 for k = 0, . . . , 9 and u (k) = −1 for k = 10, . . . , 19. Write a program that implements a predictive Kalman filter capable of estimating the position x1 (k). 2) Draw the confidence ellipses associated with the probability η = 0.99. How does the uncertainty evolve for x1 in function of k? 3) In function of k, draw the determinant of the covariance matrix Γx . Discuss. E XERCISE 7.16.– State estimation of the inverted rod pendulum We consider an inverted rod pendulum whose state equations are given by: 





x˙ 1    x˙ 2   =    x˙ 3    x˙ 4

x3 x4 m sin x2 (g cos x2 − *x24 ) + u M + m sin2 x2 sin x2 ((M + m)g − m*x24 cos x2 ) + cos x2 u *(M + m sin2 x2 )



 " #  x1   and y = x2  

. Here, we have taken as state vector x = x, θ, x, ˙ θ˙ , where the input u is the force exerted on the cart of mass M , x is the position of the cart and θ is the angle between

292

Mobile Robotics

the pendulum and the vertical direction. We will assume here that only the position of the cart x and the angle θ of the pendulum are measured. 1) Linearize this system around the state x = 0. 2) Suggest a state feedback controller of the form u = −K · x + h w that stabilizes the system. Use a pole placement method to achieve this. All the poles will be equal to −2. For the precompensator h, take a setpoint w that corresponds to the desired position for the cart. Following [JAU 15], we must take: / 0−1 h = − E · (A − B · K)−1 · B

where E is the setpoint matrix given by: E = (1 0 0 0)

Simulate the system controlled by this state feedback. 3) In order to perform an output-based feedback, we need an estimation (ˆ x of the state vector x). For this, we will use a Kalman filter (see Figure 7.12):

Figure 7.12. Kalman filter used to estimate the state of the inverted rod pendulum

Discretize the system using steps of dt = 0.1 sec, then propose a Kalman filter for observing the state. 4) Implement this filter and study the robustness of the Kalman observer when the measurement noise is increased.

Kalman Filter

293

5) An extended Kalman filter can be obtained by replacing, in the prediction step of the Kalman filter, the statement: x ˆk+1|k = Ak x ˆk|k + Bk uk by: / 0 x ˆk+1|k = x ˆk|k + f x ˆk|k , uk · dt.

Here we have replaced the prediction performed on the linearized model by a prediction performed on the initial nonlinear model that is closer to reality. Propose an implementation of this extended Kalman filter on the inverted rod pendulum. E XERCISE 7.17.– Dead reckoning Dead reckoning corresponds to the problem of localization in which only proprioceptive sensors are available. This type of navigation was used by early navigators who were trying to locate themselves during long journeys. They were able to do this in a very general way by measuring the heading of the boat, the speed at various instants and integrating all the corresponding variations in position over the entire journey. In a more general context, we may consider that using a state observer in prediction mode and without correction (in the particular case in which the state is the position of the robot) corresponds to dead reckoning. Let us consider the robot represented on Figure 7.13 and whose state equations are:     v cos δ cos θ x˙  y˙   v cos δ sin θ       θ˙  =  v sin δ + α  θ    3  v˙   u1 + αv  δ˙ u2 + α δ

where αθ , αv , αδ are independent continuous-time Gaussian white noises. In a more rigorous way, these are random distributions with infinite power, but once they are discretized, the mathematical difficulties disappear. The robot is equipped with a compass that returns θ with high precision and an angle sensor that returns the angle δ of the front wheel. 1) Discretize this system with an Euler method. Simulate this system for an arbitrary input u (t) and initial vector. For the variance of the discretized noises αθ , αv , αδ we will take 0.01 · dt, where dt is the discretization step. 2) Express this localization problem in a linear and discretized form.

3) Using a Kalman filter, predict the position of the robot as, well as the associated covariance matrix.

294

Mobile Robotics

4) How does the localization program change if we assume that, using odometers, the robot is capable of measuring its speed v with a variance of 0.01?

Figure 7.13. Dead reckoning for a tricycle robot

E XERCISE 7.18.– Goniometric localization Let us consider once again a robot vehicle described by the state equations: 

   x4 cos x5 cos x3 x˙ 1  x˙ 2   x4 cos x5 sin x3      x4 sin x5  x˙ 3  =       3  x˙ 4    u1 x˙ 5 u2

The vector (x1 , x2 ) represents the coordinates of the center of the robot, x3 is the heading of the robot, x4 its speed and x5 the angle of its front wheels. The robot is surrounded by point landmarks m(1), m(2), . . . whose positions are known. The robot can only detect these landmarks m(i) if the distance to them is sufficiently small (smaller than 15 m). In such a case, the robot measures the bearing angle δi with high precision. We will also assume that the robot knows the angles x3 and x5 at all times, without any error. Finally, it measures its speed x4 with an error of variance 1. Figure 7.14 illustrates a situation in which two landmarks m(1) and m(2) are detected by the robot. In order for the robot to locate itself, we would like to use a Kalman filter. For this, we need linear equations, which we do not have here. Since x3 and x5 are known, the nonlinearity can be based on a temporal dependency. Let us take for this z = (x1 , x2 , x4 ). 1) Show that z satisfies a linear state evolution equation. Find the associated observation equation. 2) Find a discretization for the evolution of z in order to feed a Kalman filter.

Kalman Filter

295

3) Implement a simulator with the robot surrounded by the following four landmarks: " # " # " # " # 0 15 30 15 a (1) = , a (2) = , a (3) = , a (4) = 25 30 15 20

Figure 7.14. Goniometric localization

As stated above, the robot can only goniometrically detect landmarks once they are close. 4) Implement a Kalman filter for the localization. The initial state will be assumed unknown. 5) We now have two robots Ra and Rb capable of communicating wirelessly while measuring the landmark angles (see Figure 7.15). When the distances are small (i.e. smaller than 20 m), the robots can measure the angles ϕa and ϕb with high precision using cameras (see figure). Suggest a centralized Kalman filter for the localization of the two robots. E XERCISE 7.19.– Following a boat with two radars The movement of a boat that we are seeking to follow is described by the state equations:  px (k + 1)    vx (k + 1) py (k + 1)    vy (k + 1)

= = = =

px (k) + dt · vx (k) vx (k) − dt · vx (k) + αx (k) py (k) + dt · vy (k) vy (k) − dt · vy (k) + αy (k)

296

Mobile Robotics

where dt = 0.01 and αx and αy are Gaussian white noises with variance matrix dt. The state vector is therefore x = (px , vx , py , vy ).

Figure 7.15. Goniometric localization for two communicating robots

1) Write a program that simulates the boat. 2) Two radars placed at a : (ax , ay ) = (0, 0) and b : (bx , by ) = (1, 0) measure the square of the distance to the boat. The observation equation is: yk =

"

2 2# (px (k) − ax ) + (py (k) − ay ) + βk 2 2 (px (k) − bx ) + (py (k) − by ) 2 34 5 g(xk )

where β1 (k) and β2 (k) are independent Gaussian white noises with a variance equal 1 to dt · 10−2 . Adjust the simulation in order to visualize the radars and generate the measurement vector y (k). 3) Linearize this observation equation around the current estimation x ˆk of the state vector xk . Deduce from this an equation of the form zk = Ck xk where zk = h (yk , x ˆk ) takes the role of the measurement taken at time k. 4) Implement a Kalman filter that allows the localization of the boat. E XERCISE 7.20.– Robot localization in a pool Consider an underwater robot moving within a rectangular pool of length 2Ry and width 2Ry . A sonar placed just above the robot rotates with a constant angular speed. The depth z is easily obtained using a pressure sensor and we will, therefore, assume this quantity to be known. The robot is weighted in such a way that the bank

Kalman Filter

297

and elevation angles may be assumed zero. In our context, localizing the robot means estimating the coordinates (x, y) of the robot. The origin of the coordinate system will be middle of the pool. For this localization, we will assume that the angle α of the sonar is measured relative to the body of the robot, the heading angle θ is measured with a compass and the tangential aT and normal aN accelerations with accelerometers. Every 0.1s, the sonar returns the length * of the sonar beam. Figure 7.16 represents the length *(t) of the sonar beam, obtained by simulation when the sonar performs seven rotations around itself while the robot is moving.

Figure 7.16. Telemetric measurements collected by the robot

1) Given the signal collected by the sonar, suggest a robust method for detecting local minima that correspond to the situation where the sonar is perpendicularly pointing towards one of the four walls of the pool. 2) Let vx = x, ˙ vy = y. ˙ Show that we can write the following state equations to connect the measurements:    x˙ = vx  y˙ = vy v˙ x = aT cos θ − aN sin θ    v˙ y = aT sin θ + aN cos θ

3) Let us now assume that we have collected (either by simulation or real experience) for each t ∈ [0, tmax ], data relative to the accelerations (aT , aN ), to the heading angle θ and to the angle of the sonar α. Propose a recursive method based on the Kalman filter in order to localize the robot.

298

Mobile Robotics

7.8. Corrections C ORRECTION FOR E XERCISE 7.1.– (Gaussian distribution) 1) We draw the graph of πx and we then obtain the results illustrated by Figure 7.17.

Figure 7.17. Probability density of a non-centered unit Gaussian random vector

2) We obtain the characteristics of the law πy using the following relations: y ¯ = A¯ x+b Γy = A · Γx · A T Figure 7.18 illustrates the obtained result.

Figure 7.18. Density of the random vector y after transformation of x by a linear application

Kalman Filter

299

Notice that, in accordance with the linear relation that connects x and y, πy can be obtained from πx by an expansion of 2 with respect to x1 followed by a rotation of π 6 and finally translation. The Gaussian character is preserved. C ORRECTION FOR E XERCISE 7.2.– (Confidence ellipses) The matrices Γ1 , . . . , Γ6 are drawn on Figure 7.19. Given the instructions Γ1 = I2×2 and Γ2 = 3Γ1 , the ellipses associated√with Γ1 and Γ2 are circles and Γ2 is obtained from Γ1 by a homothety with ratio 3. Since Γ3 = A1 · Γ2 · AT1 + Γ1 we can obtain Γ3 by an expansion with respect to x2 with ratio 3 followed by an inflation of the obtained ellipse, given the addition of Γ1 . To obtain Γ4 , we subject Γ3 to a rotation of angle π4 (since Γ4 = A2 · Γ3 · AT2 ). As Γ5 = Γ4 + Γ3 , Γ5 encloses ellipses Γ4 and Γ3 . Once again, Γ6 is obtained by a rotation of angle π4 of Γ5 .

Figure 7.19. Confidence ellipses associated to Γ1 , . . . , Γ6 , from thinnest to thickest

C ORRECTION FOR E XERCISE 7.3.– (Confidence ellipse: prediction) 1) We got Figure 7.20, Left, using the following program # " # 4 3 1 ¯ := ;x 3√ 3 2 ¯, . . . , x ¯ ) + Γx · randn2×N 2 X := (¯ x, x 3 Plot(X) 1 N := 1000; Γx :=

"

300

Mobile Robotics

Figure 7.20. Left Cloud of points, Right: confidences ellipses

2) We call D RAW E LLIPSE(¯ x, Γx , η) for η ∈ {0.9, 0.99, 0.999} (see section 7.22) and we obtain Figure 7.20, Right which shows the consistency between the cloud X and the ellipses. The frame box is [−10, 10]×2 . 3) Using the following statements: 1 2 3

P ˆ := N1 i X:,i ; x @ := X − x ˆ · 11×N X 1 @ ·X @T ˆ Γx := · X N

¯ ,Γx : We got the following estimates for x " # " # 0.944 3.901 2.984 ˆ ˆ= x , Γx = 1.982 2.984 3.066

4) The simulation program below generates Figure 7.21. The frame box in [−20, 20]×2 .. # 1 dt "−dt 1# " # 4 3 1 ¯ := N := 1000; Γx := ;x 3√ 3 2 ¯, . . . , x ¯ ) + Γx · randn2×N X := (¯ x, x For t = 0 : dt : 5 " # 2 u := sin(t) · dt · 3 √ X := A · X + u · 11×N + dt · randn2×N

1 dt := 0.01; A := 2 3 4 5 6

"

Kalman Filter

301

Figure 7.21. Cloud of points generated by simulation with t ∈ {0, 1, 2, 3; 4, 5}.

5) The confidence ellipses are calculated as follows: " # 1 dt 1 dt := 0.01; A := ; Γα := dt · I2×2 −dt" 1# " # 4 3 1 ¯ := 2 Γx := ;x 3 3 2 3 For t := 0 : dt : 5 " # 2 ¯ := A · x ¯ + sin(t) · dt · 4 x 3 5 Γx := A · Γx · AT + Γα Some of them are depicted on Figure 7.22. The previous instructions require a negligible amount of time compared to what we need for computing with the particle cloud. In a linear Gaussian context, we will, therefore, directly manipulate the covariance matrices and the mean values, which will lead us to the Kalman filter. In a nonlinear context, manipulating the particle cloud will often be preferred. C ORRECTION FOR E XERCISE 7.4.– (Confidence ellipse: correction) 1) The cloud is generated by the following program:

302

Mobile Robotics

# " # 3 1 1 ¯ := ;x 1√ 3 2 ¯, . . . , x ¯ ) + Γx · randn2×N 2 X := (¯ x, x 3 plot(X) 1 N := 1000; Γx :=

"

1 2) We have x ˆ1 = x ¯1 + K · (x2 − x ¯2 ) with K = Γx1 x2 Γ−1 x2 = 3 . Thus

x ˆ1 = 1 +

1 · (x2 − 2) 3

1 3) We have x ˆ2 = x ¯2 + K · (x1 − x ¯1 ) with K = Γx1 x2 Γ−1 x1 = 3 . Thus

x ˆ2 = 2 +

1 · (x1 − 1) 3

Figure 7.22. Confidence ellipsoids for t ∈ {0, 1, 2, 3; 4, 5}.

To correctly understand the non-invertibility of estimators, we represent them graphically as shown in Figure 7.23. We then invoke the estimator in one direction and then in the other. We do not return to the same point. C ORRECTION FOR E XERCISE 7.5.– (Covariance matrix propagation) 1) We have: x ¯ = E (x) = E (A a − b) = AE (a) − E (b) = 0

Kalman Filter

since the random variables a and b are centered. Similarly: y¯ = E (y) = E (C x + c) = CE (x) + E (c) = 0 since c is centered. For the covariance matrices, we have: . / 0 T Γx = E (x − x ¯) (x − x ¯ ) = E x xT . T = E (A a − b) (A a − b) / 0 = E A a a T AT − A a b T − b a T AT + b b T / 0 / 0 / 0 / 0 = A E a a T AT − A E a b T − E b a T AT + E b b T 2 34 5 2 34 5 2 34 5 2 34 5 =I

=0

=0

=I

T

= A · A + I.

Figure 7.23. The two lines in bold represent the two estimators x ˆ1 (x2 ) and x ˆ2 (x1 )

Similarly: / 0 Γy = C Γx CT + Γc = C A AT + I CT + I.

2) We have of course v ¯ = (¯ x, y ¯) = 0. This gives: " / T0 / T0 # / 0 E /x x 0 E /x y 0 Γv = E v v T = E y xT E y y T

303

304

Mobile Robotics

However . / 0 / 0 T Γxy = E x yT = E x (C x + c) = E xxT CT + xcT = Γx CT + Γxc = Γx CT

since x and c are independent (and thus Γxc = 0). It follows that Γv =

"

#

.

−I I

#

Γ x Γ x CT C Γx Γy

3) We have: z = (−I I) · v Therefore: Γz = (−I I) Γv = (−I I)

"

"

= (−I I)

−Γx + Γx CT −C Γx + Γy

#

"

Γ x Γ x CT C Γ x Γy

#"

−I I

#

= Γ x − Γ x CT − C Γ x + Γ y

4) Following the expression of the linear estimator, we have x ˆ = x ¯ + K@ y with T −1 @ K = Γxy Γ−1 .= Γ C Γ and y = y − C¯ x . Therefore: x y y / 0 x ˆ=x ¯+ Γx CT Γ−1 (y − C¯ x) = Γx CT Γ−1 y y ·y

since x ¯ = 0.

C ORRECTION FOR E XERCISE 7.6.– (Brownian noise) 1) Let us first of all recall that if a and b are two independent centered random variables with variance σa2 and σb2 , and if α is a deterministic real number then: . / 0 2 2 σa+b = E (a + b) = E a2 + b2 + 2ab = σa2 + σb2 + 2E (ab) = σa2 + σb2 . 2 2 σαa = E (αa) = α2 σa2 It follows that:

σy2 (tk ) = δ 2 ·

k 1 j=0

σx2 (tj ) = δ 2 · kσx2 .

Kalman Filter

305

However tk = kδ and therefore σy2 (tk ) = δtk σx2 . We can thus see that the √ √ standard deviation σ√y (tk ) = δ tk σx tends towards zero when δ → 0 and that this error evolves with t (due to the random walk). This phenomenon comes from the fact that the errors compensate for each other, even more so as δ is small. In order to illustrate this phenomenon, we write a simulation function as follows:

1 2 3 4 5 6

Function S IMU (δ, σx , tmax ) T := (0, δ, 2δ, . . . , tmax ) kmax := tmax δ ; Y (0) := 0 For k := 0:kmax X(k) := σx · randn(1) Y (k + 1) := Y (k) + δX(k) Return (T, X, Y )

We then apply this function for δ = 0.1, δ = 0.01 and δ = 0.001. The obtained result is represented on Figure 7.24. Each figure corresponds to t ∈ [0, 100] and y ∈ [−7, 7]. Notice that when the sampling period δ decreases, the Brownian noise becomes smaller due to the compensation effect. The figures were generated using the program below: 1 For i := 0 : 100 2 δ := 0.1 (or 0.01, 0.001) 3 σx := 1; tmax := 100 4 (T, X, Y ):=S IMU (δ, σx , tmax ) 5 Plot(T, Y ) 2) We have 1 σy (tk ) σx (δ) = √ · √ tk δ In order to maintain σy (tk ) independent of δ, the standard deviation σx for x (tk ) must therefore increase to infinity in order to decrease the compensation effect. When we reach the limit, i.e. δ = 0, the signal x (tk ) has an infinite standard deviation. It is for this reason that a continuous-time centered white random signal (with δ = 0) has to have an infinite power in order to have influence over an integrator or, more generally, over any physical system. We can observe this phenomenon in physics when we study the Brownian motion of particles moving within a limited space with an infinite speed. In such as case, y (t) corresponds to the movement of the particle and x (t) to its speed. Figure 7.25 gives the white noise for different values of δ, together with their integrals. The scales are identical to those chosen for Figure 7.24. We can see that when δ tends towards 0, the power of the white noise x (t) must be increased in order to obtain a similar Brownian noise. The figures were generated with the following program:

306

Mobile Robotics

1 For i := 0 : 100 2 δ := 10 (or 1, 0.1, 0.01) 3 σx := √1δ ; tmax := 100 4 (T, X, Y ):=S IMU (δ, σx , tmax ) 5 Plot(T, X); Plot(T, Y )

Figure 7.24. When δ approaches 0, the Brownian noise (integral of the white noise) becomes smaller

Figure 7.25. White noise with its corresponding Brownian noise for different values of δ

Kalman Filter

307

C ORRECTION FOR E XERCISE 7.7.– (Solving three equations using a linear estimator) We translate the problem as follows:       8 2 3 " # β1 x 1 7 = 3 2 +  β2  x 0 1 −1 2 342 5 β3 2 34 5 2 34 5 x 2 34 5 y

C

β

We take:

  " # " # 1 0 0 0 1000 0 x ¯= , Γx = , Γβ =  0 4 0  0 0 1000 0 0 4

which is equivalent to saying that a priori the vector x is more or less within the interval [−33, 33], that the βi are independent of one another and that the first equation is twice as accurate. We obtain:   8 @ = y − C¯ y x = 7 0   13001 12000 −1000 Γy = CΓx CT + Γβ =  12 000 13 004 1000  −1000 1000 2004 " # −0.09 0.288 0.311 K = Γx CT Γ−1 = y " 0.355 # −0.155 −0.24 1.311 x ˆ = x ¯ + K@ y= 1.756 " # 0.722 −0.517 Γε = Γx − KCΓx = −0.54 0.44

We can thus represent the solution of our linear system by x ˆ and the matrix Γε . C ORRECTION FOR E XERCISE 7.8.– (Estimating the parameters of an electric motor using a linear estimator) Let us apply the formulas in [7.10] with     4 0 5  10   10 1  " # " #     1 40  10 5  , Γβ = 9 · I5×5 and y =  x ¯= , Γx = ;C =     8  −1 04  14   13 5  17 15 3

308

Mobile Robotics

We obtain:   1 1    @ = y − C¯ y x= 3 6 5 

73 160 160  160 413 420  Γy = CΓx CT + Γβ =   160 420 509  208 540 620 240 612 660 " 0.027 0.0491 K = Γx CT Γ−1 y = −0.0739 −0.118 " # 1. 2 x ˆ=x ¯ + K@ y= −0.58 " 0.062 −0.166 Γε = Γx − KCΓx = −0.179 0.593

208 540 620 785 840

 240 612   660   840  945

−0.0247 −0.0044 0.046 0.148 0.092 −0.077 #

C ORRECTION FOR E XERCISE 7.9.– (Trochoid) 1) We have: " # " 2 # 0 10 0 p ¯= , Γp = 0 0 102      0.38 1 − cos (1) 1  3.25   1 − cos (2)   −2  0    y=   4.97  , C =  1 − cos (3)  , Γβ = 10  0 −0.26 1 − cos (7) 0

and we apply the linear estimator. We obtain: p ˆ=

"

2.001 2.999

#

and Γε =

"

0.0025 −0.0001 −0.0001 0.0050

2) The estimated path of the mass is: " # " # " # x t sin t = pˆ1 − pˆ2 y 1 cos t as illustrated by Figure 7.26.

#

0 1 0 0

0 0 1 0

 0 0  0 1

#

Kalman Filter

309

Figure 7.26. Estimated path for the mass

C ORRECTION FOR E XERCISE 7.10.– (Solving three equations using a Kalman filter) 1) The required script is the following: 1 Γα :="02×2 # ; A :="I2×2 # 0 0 ˆ := 2x ; u := ; Γx := 1000 · I2×2 0 /0 0 3 C := 2 3 ;Γβ := 1; y := 8 3 (ˆ x, Γx/) :=K0ALMAN (ˆ x, Γx , u, y, Γα , Γβ , A, C) C := 3 2 ;Γβ := 4; y := 7 (ˆ x, Γx/) :=K ALMAN (ˆ x, Γx , u, y, Γα , Γβ , A, C) 0 C := 1 −1 ;Γβ := 4; y := 0 (ˆ x, Γx ) :=K ALMAN (ˆ x, Γx , u, y, Γα , Γβ , A, C) 2) If we draw the three associated covariance matrices we realize that at each new iteration, the covariance matrix contracts until it is concentrated around a single point, which corresponds to the solution. 3) As expected, the obtained results are identical to those obtained in exercise 7.7. C ORRECTION FOR E XERCISE 7.11.– (Three-step Kalman filter) Figure 7.27 gives the confidence ellipses obtained by the Kalman filter.

310

Mobile Robotics

Figure 7.27. Graphical illustration of the Kalman filter on a simple example

The corresponding program is the following: " # 0 ˆ := 1 Γα := I2 ; Γβ := 1; x ; Γx := 100 · I2 0 " # " # / 0 0.5 0 8 2 A0 := ;C0 := 1 1 ;u0 := ; y0 := 7 0 1 16 3 (ˆ x, Γx )":=K ALMAN x, Γx , u0 , y0 , Γα , Γ" β , A0 , C # (ˆ #0 ) / 0 1 −1 −6 4 A1 := ;C1 := 1 1 ;u1 := ; y1 := 30 1 1 −18 5 (ˆ x, Γx )":=K ALMAN x, Γx , u1 , y1 , Γα , Γ" β , A1 ,#C1 ) # (ˆ / 0 1 −1 32 6 A2 := ;C2 := 1 1 ;u2 := ; y2 := −6 1 1 −8 7 (ˆ x, Γx ) :=K ALMAN (ˆ x , Γ x , u 2 , y 2 , Γ α , Γ β , A 2 , C2 ) C ORRECTION FOR E XERCISE 7.12.– (Estimating the parameters of an electric motor) In order to use the Kalman filter, we will take the state equations:  " # 1 0   x = x k + uk + α k  k+1  0 1   2 34 5 / Ak 0   yk = U (k) Tr (k) xk + βk    2 34 5  Ck

Kalman Filter

311

The associated program is:

1

2 3 4



   4 0 5  10 1   10         C :=   10 5  ; y :=  11   13 5   14  15 3 " # " # 17 1 0 ˆ := x ; u := ; Γx := 4 · I2 ; Γα := 02 ; Γβ := 9; A := I2 −1 0 For k := 1 to 5 (ˆ x, Γx ) :=K ALMAN (ˆ x, Γx , u, y(k), Γα , Γβ , A, C(k, :))

The obtained estimation is then: " # " # 1.13 0.06 −0.17 ˆ= x and Γx = −0.14 −0.17 0.6 C ORRECTION measurements)

FOR

E XERCISE

7.13.–

(Localization

from

wall

distance

1) We have: d (i) = −u2 (i) · x1 + u1 (i) · x2 + u2 (i) · a1 (i) − u1 (i) · a2 (i) + βi By taking yi = d (i) − d¯(i) and d¯(i) = u2 (i) · a1 (i) − u1 (i) · a2 (i), we obtain / 0 yi = −u2 (i) u1 (i) x + βi

Thus, we form the quantities: 

   d (1) − d¯(i) −u2 (1) u1 (1) y =  d (2) − d¯(i)  , C = −u2 (2) u1 (2)  , d (3) − d¯(i) −u2 (3) u1 (3) " # 1 Γβ = I3 , Γx = 100 · I2 , x ¯= 2 We then apply the equations of the orthogonal linear estimator: @ @ = y − C¯ x ˆ=x ¯+K·y y x Γy = CΓx CT + Γβ Γε = (I − KC) Γx K = Γx CT Γ−1 y

312

Mobile Robotics

2) The requested program is: # " # 2 15 3 15 3 2 ; B := ; C := ∅; d := ∅ 1 5 12 5 12 1 For i := 1 to 3 a := ith-column(A); b ="ith-column(B); # u1 a1 b−a u := &b−a& ; di := − det u2 a# 2 " # " C d C := ; d := −u2 u1 di   " # " # 2 1 0 x0 := ; Γ0 := 102 · I2 ; u := ; y :=  5  − d; Γα := 02 ; Γβ := I3 2 0 4 (x1 ,Γ1 ) :=K ALMAN (x0 , Γ0 , u, y, Γα , Γβ , I2 , C)

1 A := 2 3 4 5 6 7

"

The estimation thus calculated is represented on Figure 7.28 together with its confidence ellipse.

Figure 7.28. Confidence ellipse (in bold) associated with the localization of the robot

Kalman Filter

313

C ORRECTION FOR E XERCISE 7.14.– (Temperature estimation) 1) The Kalman filter is given by:  x ˆk+1|k     Γ  k+1|k    ˆk|k  x Γk|k   y@k     S  k   Kk

=x ˆk|k = Γk|k + Γα =x ˆk|k−1 +Kk y@k = (1 − Kk ) Γk|k−1 = yk − x ˆk|k−1 = Γk|k−1 + Γβ = Γk|k−1 Sk−1

Or equivalently  / 0 Γk|k−1 x ˆk+1|k = x ˆk|k−1 + Γk|k−1 yk − x ˆk|k−1   +Γ β  0 Γk|k−1 / =x ˆk|k−1 + Γk|k−1 yk − x ˆk|k−1 +3 .   Γk|k−1 Γ = 1− Γ +Γ = 1− k+1|k

Γk|k−1 +Γβ

k|k−1

α

Γk|k−1 Γk|k−1 +3

.

Γk|k−1 + 4

2) For k → ∞, we have Γk+1|k − Γk|k−1 → 0, i.e. Γk+1|k → Γ∞ . Therefore: Γ∞ =

" 1−

Γ∞ Γ ∞ + Γβ

#

Γ∞ + Γα

i.e.: Γ2∞ − Γα Γ∞ − Γα Γβ = 0 A single positive solution exists. It is given by: : √ Γα + Γ2α + 4Γα Γβ 4 + 16 + 4 · 4 · 3 Γ∞ = = =6 2 2

and therefore the asymptotic filter is expressed as: x ˆk+1 = x ˆk +

2 (yk − x ˆk ) . 3

The precision of the estimation√is given by the variance Γ∞ = 6. We will have the temperature with a precision of ± 6 deg.

3) In the situation where Γαk = 0, we obtain Γ∞ = 0. This means that after a sufficiently long amount of time, the non-asymptotic Kalman filter returns the correct temperature, without any uncertainties.

314

Mobile Robotics

C ORRECTION FOR E XERCISE 7.15.– (Blind walker) 1) We have a linear evolution: " # 1 u (k) x (k + 1) = x (k) 0 1

The program that simulates our system and estimates the state using a Kalman filter is the following: - . . . . 0 0 0 0 0 0 ; Γx := ; Γα := ; x := −2 −2 1 0 2 · 10 0 10 1 + 0.02 · randn(1) For k = 0 to 19 if k < 10, then.u := 1 else - u :=.−1; 1 u 1 0 A := ; C := 0 1 0 1 (ˆ x,Γx ) :=K ALMAN (ˆ x, Γx , 0, ∅, Γα , ∅, A, C) x := A · x + randn (Γα )

1 x ˆ := 2 3 4 5 6

2) The ellipses are given on top of Figure 7.29. Since we have no exteroceptive measurements, we notice that these ellipses are growing. However, as the standard deviation shows in function of time, the projection of these ellipses according to x1 forms intervals whose size may decrease. Indeed, the accumulated uncertainties of the scale factor on the forward journey are partly recovered on the way back. If one takes ten steps forward and ten steps back with equal length one returns to the initial position, regardless of the length of the individual steps. N OTE.– In a commercial underwater robot using dead reckoning for navigation (localization using a Kalman predictor), if the scale factors are not well-known a decreasing uncertainty can often be observed on the return journey. This is the same phenomenon as the one described in this exercise with the walker. C ORRECTION FOR E XERCISE 7.16.– (State estimation of the inverted rod pendulum) 1) Let us linearize this system around x = 0 using the Taylor-Young method. We have: m sin x2 (g cos x2 − *x24 ) + u = (x2 +ε)(g(1+ε)−)ε)+u 2 M + m sin2 x2 M + m (x2 + ε) m (x2 + ε) (g + ε) + u mgx2 + u = = + ε. M +ε M 2 sin x2 ((M + m)g − m*x4 cos x2 ) + cos x2 u *(M + m sin2 x2 ) (x + ε) ((M + m)g − m*ε (1 + ε)) + (1 + ε) u = 2 2 *(M + m (x2 + ε) ) x2 (M + m)g + u = + ε. *M

Kalman Filter

315

Figure 7.29. Top: confidence ellipses for k from 0 to 19; bottom: standard deviation for x1

We thus obtain the linearized system:   0 0      0 0    x˙ =  mg   0   M  +m)g  0 (MM ) 2 34  A  "    1 0 0   y =   0 1 0   2 34  C

1 0 0 0 0 0

#

  0 0  0 1 x +  1  0 M 1 0 M) 2 34 5 B

x



 u  5

5

2) The gain K is obtained by solving the system: 4

det (A − BK) = (s + 2) . The result can be calculated by the M ATLAB® or P YTHON (using roblib.py) statement: K=place(A,B,[-2 -2.01 -2.02 -2.03]);

316

Mobile Robotics

where we have taken care to avoid multiples poles in order for the place function to work. The precompensator is obtained by the relation:

where:

/ 0−1 h = − E · (A − B · K)−1 · B / 0 E= 1 0 0 0 .

The initialization part of the program is 1 m :=  1;M := 5;l := 1;g :=9.81   0 0 1 0 0 " # 0   0 0 1 ; B :=  01  ; C := 1 0 0 0 2 A :=  mg 0   0 0 0 1 0 0 M M (M +m)g 1 0 0 0 M) M) 3 K := place(A, B, (−2, −2.1, −2.2, −2.3)) / 0 / 0−1 4 E := 1 0 0 0 ; h = − E · (A − B · K)−1 · B

The program for the simulation is of the form:

5 6 7 8



 0  0.1   x :=   0  ; w := 1; dt := 0.1 0 For t := 0 : dt : 10 u := −K · x + h · w x := x + f (x, u) · dt

3) The Euler discretization yields: x (k + 1) = (I+dt A) · x (k) + dt B u (k) + α (k) where the vector α (k) ∈ R4 is the state noise that takes into account the errors due to modeling and discretization. The observation equation is: "

# " # 1 0 0 0 β1 (k) y (k) = x (k) + . 0 1 0 0 β2 (k) 2 34 5 C

To have a behavior of the simulation which is independent of the sampling time 1 . This will be dt, we need to take Γα proportional to dt and Γβ proportional to dt explained in the section 7.5 in the next chapter.

Kalman Filter

317

4) The program is the following: 1 5 5 Γx := · 10−5 · I4 ; Γβ := dt · 10−5 · I2 10 ·I4 ; Γα :=  dt 0 0  0.1  0  ˆ :=   ; w := 1; dt := 0.1 6 x :=   0 ; x 0 0 0 7 For t = 0 : dt : 10 ˆ+h·w 8 u := −K · x 9 y := C · x + randn(Γβ ) 10 (ˆ x,Γx ) :=K ALMAN (ˆ x, Γx , dt · B · u, y, Γα , Γβ , I4 + dt · A, C) 11 x := x + dt · f (x, u) + randn(Γα )

C ORRECTION FOR E XERCISE 7.17.– (Dead reckoning) 1) The difficulty lies in choosing the covariance matrix and drawing the random noise. The simulation is performed using a simple Euler method: 

  0 0  0  0 " #  π   0 −2 −2   x :=   3  ; dt := 10 ; u := 0 Γα := dt · 10  0  4  0 0.3 0 For t = 0 : dt : 1 x := x + dt · f (x, u) + randn(Γα )

0 0 0 0 0

0 0 1 0 0

0 0 0 1 0

 0 0  0  0 1

2) In order to use the Kalman filter to predict the state of the system, we need to describe the dependencies between the state variables using a linear state equation. If we let z = (x, y, v), we obtain: 

     0 0 cos δ cos θ 0 0 z˙ =  0 0 cos δ sin θ  z +  0  +  0  0 0 0 u1 α2

And, after discretization by Euler’s method:

zk+1

     0 1 0 dt cos δ cos θ 0 + 0  0 =  0 1 dt cos δ sin θ  zk +  dt · α2 0 0 1 dt · u1 (k) 34 5 2 34 5 2 34 5 2 

=Ak

=uk

=αk

318

Mobile Robotics

3) The program is:    0 0 0 0 0 0 0 0 0  0   π   −2   x :=   3  ; dt := 0.01; Γα := dt · 10  0 0 1 0  4  0 0 0 1 0.3 0 0 0 0   x1 ˆ z :=  x2  ; Γz = dt · 10−2 · I3 ; x4 For t = 0 : dt : 10 T T u = (0,0) ; uz = (0, 0, dt · u1 )  1 0 dt · cos x3 · cos x5 Ak =  0 1 dt · sin x3 · cos x5  0 0 1 Γβ = ∅; y = ∅; Ck = ∅ (ˆ z,Γz ) :=K ALMAN (ˆ z, Γz , uz , y, Γz , Γβ , Ak , Ck ) x := x + dt · f (x, u) + randn(Γα )

 0 0  0  0 1

4) If we would have had odometers capable of giving an approximation of the speed with a variance of 0.01, we would have had to take following values as observation: Γβ = 0.01; y = x4 + randn(Γβ ); C = (0, 0, 1) C ORRECTION FOR E XERCISE 7.18.– (Goniometric localization) 1) We have:       x˙ 1 x4 cos x5 cos x3 z˙1  z˙2  =  x˙ 2  =  x4 cos x5 sin x3  x˙ 4 u1 z˙3      0 0 0 cos x5 cos x3 z1 =  0 0 cos x5 sin x3   z2  +  0  z3 u1 0 0 0

When the robot detects the landmark m (i) = (xm (i) , ym (i)) with an angle δi , we have: i.e.:

(xm (i) − x1 ) sin (x3 + δi ) − (ym (i) − x2 ) cos (x3 + δi ) = 0 −xm (i) sin (x3 + δi ) + ym (i) cos (x3 + δi ) 2 34 5 known

/

0

= − sin (x3 + δi ) cos (x3 + δi ) 2 34 5 known

"

x1 x2

#

+ βi

Kalman Filter

319

where βi is a noise that we can assume to be white Gaussian with variance 1. This noise allows us to take into account the uncertainties on the measured angles (mainly the δi ). If {i1 , i2 , . . . } are the numbers of the landmarks detected by the robot, we have the observation equation:   0 0 1  − sin (x3 + δi1 ) cos (x3 + δi1 ) 0    y (k) =  − sin (x3 + δi ) cos (x3 + δi ) 0  · z (k) 2 2   .. .. .. . . . 2 34 5 C(k)

Note that the dimension of y depends on k. The first equation is given by the odometers that give us the speed. The other equations correspond to the goniometric measurement of the landmarks. 2) An Euler discretization yields:     1 0 dt · cos x5 · cos x3 0 z (k + 1) =  0 1 dt · cos x5 · sin x3  · z (k) +  0  + α (k) 0 0 1 dt · u1 2 34 5 A(k)

which is linear.

3) To design this simulator, we take the simulator in exercise 7.18, to which we need to add an observation function. In order to feed the Kalman filter, this function must return a vector of measurements y, the associated covariance matrix, but also the observation matrix C (k). This observation function is: Function g(x) 1 Ck := (0, -0, 1); . y := x4 ; Γ -β :=.1 - . - . 0 15 30 15 2 a (1) := ; a (2) := ; a (3) := ; a (4) := 25 30 15 20 3 For i := 1 : 4, 4 δ˜ := angle(a(i) − x1:2 ) 5 If (a(i) − x1:2 ( < 15 6 yi := −a1 · sin δ˜ + a2 · cos δ˜ ˜ cos δ, ˜ 0) 7 Cki := (− sin δ, 8 y := [y; yi ]; Ck := [Ck ; Cki ]; Γβ := diag(Γβ , 1) 9 y := y + randn(Γβ ) 10 Return (y, Γβ , Ck )

320

Mobile Robotics

4) The main program including the simulator and the Kalman filter is the following: 

 0    −20  - . 0  π  0 ; z ˆ :=  0 ; dt := 0.01 u := ; x :=  3   0  20  0 0.1 Γz := 103 .I3×3 ; Γα :=dt · 0.01 · I3×3 For t := 0 : dt : 10 (y, Γβ , Ck ) = g(x)  1 0 dt · cos x5 cos x3 Ak :=  0 1 dt · cos x5 sin x3  0 0 1 uk := (0, 0, dt · u1 )T (ˆ z,Γz ) :=K ALMAN (ˆ z,Γz , uk , y, Γα , Γβ , Ak , Ck ) α := randn(Γα ) x := x + f (x, u) · dt + (α1 , α2 , 0, α3 , 0)T

5) We may consider the two robots as a single system whose state vector is: x = (xa1 , xa2 , xa3 , xa4 , xa5 , xb1 , xb2 , xb3 , xb4 , xb5 ) . The vector: z = (xa1 , xa2 , xa4 , xb1 , xb2 , xb4 ) satisfies a linear evolution equation given by: 

  x˙ a1 0 0 cos xa5 cos xa3  x˙ a2   0 0 cos xa5 sin xa3     x˙ a4  0 0 0     x˙ b1  =  0 0 0     x˙ b1  0 0 0 x˙ b1 0 0 0 2 34 5 =z˙





0  0     ua1   +  0     0  ub1

0 0 0 0 0 0

  0 0 xa1   xa2  0 0     xa4  0 0     0 cos xb5 cos xb3    xb1    0 cos xb5 sin xb3 xb1  0 0 xb1 2 34 5 =z

Kalman Filter

321

When the two robots can see each other, we have the relation: (xb1 − xa1 ) sin (xa3 + ϕa ) − (xb2 − xa2 ) cos (xa3 + ϕa ) = 0 i.e.: 0 = (− sin (xa3 + ϕa ) , cos (xa3 + ϕa ) , 0, sin (xa3 + ϕa ) , − cos (xa3 + ϕa ) , 0) · z + β (k) where β (k) corresponds to a measurement noise that we can assume to be white Gaussian. The 0 on the left of the equality corresponds to the virtual measurement. The associated observation function can be coded in the following manner:

1 2 3 4 5 6

Function gab (xa , xb ) d := xb,1:2 − xa,1:2 ; ϕ˜ := atan2(d) yab := ∅; Γβab := ∅; Cab := ∅ If -d- < 20 ˜ cos ϕ, ˜ 0, sin ϕ, ˜ − cos ϕ, ˜ 0] Cab := [ − sin ϕ, Γβab := 1; yab := randn(Γβab ) Return (yab , Γβab , Cab )

To this inter-robot observation function, we need to add the detection of landmarks as already seen in the previous question. We may then merge the inter-robot observation function and that of the landmarks into a single function as follows: Function gall (xa , xb ) 1 (ya , Γβa , Ca ) := g(xa ); (yb , Γβb , Cb ) := g(xb ) 2 (yab , Γ βab , C ab ) := gab (xa , xb ); " # ya diag(Ca , Cb ) 3 y :=  yb ;Ck := Cab yab 4 Γβ := diag(Γβa , Γβb , Γβab ) 5 Return (y, Γβ , Ck )

322

Mobile Robotics

We can then use a Kalman filter to perform the localization. The script is given by:   −13  −22  " # " #  π  0 0  ua := ; ub := ; xa :=   3 ; 0 0  15  0.1     0 20 0  −10     π  0    ˆ :=  xb :=  3 ; z 0    18  0 0.2 0 Γz := 103 .I6×6 ; Γαa :=dt · diag(0.1, 0.1, 0.5); Γαb := Γαa ; Γα := diag (Γαa , Γαb ) For t = 0 : dt : 10 (y, Γβ  , Ck ) = gall (xa , xb )  1 0 dt · cos xa5 cos xa3 0 0 0  0 1 dt · cos xa5 sin xa3 0 0  0   0 0  1 0 0 0   Ak =   0 0 0 1 0 dt · cos x cos x b5 b3   0 0 0 0 1 dt · cos xb5 sin xb3  0 0 0 0 0 1 uk = (0, 0, dt · ua1 , 0, 0, dt · ub1 )T (ˆ z,Γz ) :=K ALMAN (ˆ z,Γz , uk , y, Γα , Γβ , Ak , Ck ) αa := randn(Γαa ); αb := randn(Γαb ); xa := xa + f (xa , ua ) · dt + (αa1 , αa2 , 0, αa3 , 0)T xb := xb + f (xb , ub ) · dt + (αb1 , αb2 , 0, αb3 , 0)T C ORRECTION FOR E XERCISE 7.19.– (Following a boat with two radars) 2) Given that: " 2 2# (px − ax ) + (py − ay ) g (x) = 2 2 (px − bx ) + (py − by ) we have: dg (ˆ x) = dx

"

2 (ˆ px − ax ) 0 2 (ˆ py − ay ) 0 2 (ˆ px − bx ) 0 2 (ˆ p y − by ) 0

#

And therefore the observation equation can be approximated by its tangent equation: y . g (ˆ x) +

dg (ˆ x) · (x − x ˆ) dx

Kalman Filter

323

Or equivalently, dg dg y − g (ˆ x) + (ˆ x) · x ˆ . (ˆ x) · x. 2 34 dx 5 2dx34 5 z

C

3) To implement the Kalman filter, we take 

and

1 dt  0 1 − dt Ak =  0 0 0 0

Ck =

"

 0 0 0 0   1 dt  0 1 − dt

2 (ˆ px − ax ) 0 2 (ˆ py − ay ) 0 2 (ˆ px − bx ) 0 2 (ˆ py − by ) 0

#

.

4) The program is the following:     0 0 " # " # 0 0 0 1    x :=   2  ; a := 0 ; b := 0 ; dt := 0.01; u :=  0  0   0 1 dt 0 0 0 0 0 0  0 1 − dt 0   0  ; Γ := dt ·  0 1 0 0  A :=  0 0 0 0 0 0 1 dt  α 0 1 − dt 0 0 0 1  0 0 1 0 1 5 −2  x ˆ :=   3  ; Γx := 10 · I4 ; Γβ := dt · 10 · I2 0 For t = 0 : dt : 10 y := g(x) " + randn(Γβ ) # 2 (ˆ x1 − ax ) 0 2 (ˆ x3 − ay ) 0 Ck := 2 (ˆ x1 − bx ) 0 2 (ˆ x 3 − by ) 0 ˆ zk := y − g (ˆ x) + Ck · x (ˆ x,Γx ) :=K ALMAN (ˆ x, Γx , u, zk , Γα , Γβ , A, Ck ) x := A · x + randn(Γα ) C ORRECTION FOR E XERCISE 7.20.– (Robot localization in a pool) 1) When the sonar is facing one of the walls, the returned distance * satisfies: a = * · cos β

[7.14]

324

Mobile Robotics

where β is the angle between the wall normal and the sonar beam and a is the distance between the sonar and the wall. We will assume that the robot is immobile and that only the sonar rotates (this is the same as assuming that the tangential v and angular θ˙ speeds of the robot are negligible compared to the rotation speed of the sonar α). ˙ First of all, let us consider the situation in which, at time t, the sonar is in the normal axis of the wall. If τ is a sufficiently small positive real number, i.e. such that at time t − τ the sonar points towards the same wall, then from relation [7.14] we would have: a = *(t − τ ) · cos (−ατ ˙ ). Note that the rotation speed of the sonar α˙ is assumed to be known and constant. Let us take τ = kδ, k ∈ {0, 1, 2, . . . , N − 1}, where δ is the amount of time between two pings of the sonar and N is an integer such that at time t − N δ, the sonar necessarily points towards the wall that is orthogonal to the sonar beam at time t. Let us take: @ ak = *(t − kδ) · cos (−kδ α) ˙ .

The quantity @ ak should correspond to the distance a between the robot and the wall it points towards. However, given the presence of measurement noise, it is preferable to obtain an estimation a ˆ of the distance a using an average: a ˆ=

N −1 1 1 @ ak . N k=0

We can verify the fact that the sonar is perpendicularly pointing towards a wall by verifying that the variance of the @ ak is small, i.e. N −1 1 1 2 (@ ak − a ˆ) < ε, N k=1

where ε is a fixed threshold close to 0. This is the variance test. However, in practice there is a lot of incorrect data. We, therefore, need to change our method. A more robust method than the one presented earlier consists of calculating the median instead of the mean. For this, the @ ak have to be sorted in increasing order. Next, the middle a ¯ of the list is taken and the elements of this list that are the furthest from a ¯ are removed. Let us say half of them are removed. These elements are easy to find since they are either in the beginning or at the end of the list. The average of the remaining elements is then calculated to obtain a ˆ. The variance test is performed on the remaining elements in order to verify that the sonar is in normal direction to a wall. Note that if the robot has a reliable compass, the variance test becomes useless. Indeed, the compass gives

Kalman Filter

325

us θ and the angle α is known, which allows us to know whether the sonar points or not towards the normal axis of a wall and also which wall it is. 2) For the last two equations, it is sufficient to note that the absolute acceleration is obtained from the measured acceleration (aT , aN ) by the accelerometers from a simple rotation of angle θ: " # " #" # x ¨ cos θ − sin θ aT = y¨ sin θ cos θ aN 3) In order to use a Kalman filter, we first of all need to discretize time. Note that when α + θ is a multiple of π2 , the sonar beam is facing one of the four corners of the pool (given that the latter is assumed to be rectangular). In such a case, the length measured might allow us to calculate either x or y. In our problem, the discrete time k increases whenever the sonar beam is facing one of the pool walls. Thus k = E( α+θ π/2 ), where E denotes the integer part of a real number. An Euler discretization of our state equations is the following:    x(k + 1)  y(k + 1) v  x (k + 1)   vy (k + 1)

= = = =

x(k) + vx (k) · T (k) y(k) + vy (k) · T (k) vx (k) + (aT (k) cos θ(k) − aN (k) sin θ(k)) · T (k) vy (k) + (aT (k) sin θ(k) + aN (k) cos θ(k)) · T (k)

where T (k) is the time elapsed between two consecutive increments of k. In matrix form, these state equations become:   1 0 T (k) 0 0 1 0 T (k)   x(k) x(k + 1) =  0 0 1 0  0 0 0 1   0 0 [7.15]   0 0  +  −T (k) sin θ(k) T (k) cos θ(k)  u(k) T (k) cos θ(k) T (k) sin θ(k) r(k) = C(k) · x(k) with x(k) = (x(k), y(k), vx (k), vy (k)) u(k) = (aN (k), aT (k))

326

Mobile Robotics

Concerning the measurement equation, we need to distinguish between four cases: – case 0, wall on the right (θ(k) + α(k) = 2kπ). In this case, we have a measurement of x: r(k) = x(k) . Rx − d(k), where d(k) is/ the distance 0 returned by the sonar. The observation matrix will therefore be C(k) = 1 0 0 0 ; – case 1, wall on the bottom (θ(k) + α(k) = 2kπ + π2 ). In this /case, we have a 0 measurement of y: r(k) = y(k) . Ry − d(k) and therefore C(k) = 0 1 0 0 ;

– case 2, wall on the left (θ(k) + α(k) = 2kπ + π). Once more, we have a measurement of x:0 r(k) = x(k) . −Rx + d(k). The observation matrix will be / C(k) = 1 0 0 0 ;

3π – case 3, wall in front (θ(k) + α(k) = 2kπ / + 2 ).0 We have a measurement of y: r(k) = y(k) . −Ry + d(k). Thus C(k) = 0 1 0 0 .

If we know the value of θ + α, in order to find out which case i we are in, we need to solve: ∃k ∈ N, θ(k) + α(k) = 2kπ +

iπ 2

i.e. ∃k ∈ N,

2 (θ(k) + α(k)) = i + 4k. π

Therefore, in order to have the most favorable case i, we calculate the integer closest to the quantity π2 (θ(k) + α(k)), and we look at the remainder of the Euclidean division of this integer by 4: i = mod(round(

2 (θ(k) + α(k)) , 4)). π

Let us note that the goal of the system in [7.15] is not to reproduce the dynamic behavior of the system on the control level, but to allow the utilization of a Kalman filter with the aim of estimating the position and speed of the robot. We have made sure to have a discrete system described by linear state equations of the form: ! x(k + 1) = A(k)x(k) + B(k)u(k) + α(k) r(k) = C(k)x(k) + β(k) We have just added two signals with noises α and β which are assumed to be white and with covariance matrices Γα and Γβ (note that here, Γβ is a scalar). The goal of having these two matrices is to model the uncertainties of the model and the measurement noises. A Kalman filter will then be able to help with the localization. Figure 7.30 represents the robot at a time t, the associated sonar beam and a series of confidence ellipses created by the Kalman filter. The big circle represents the initial confidence ellipse.

Kalman Filter

Figure 7.30. Localization of the underwater robot using the Kalman filter

327

8 Bayes Filter

8.1. Introduction This chapter proposes to generalize the Kalman filter to cases where the functions are nonlinear and the noise is non-Gaussian. The resulting observer will be called the Bayes filter. Instead of computing for each time k the covariance and the estimate of the state, the Bayes filter directly computes the probability density function of the state vector. As for the Kalman filter, it consists of two parts: the prediction and the correction. In the linear and Gaussian case, the Bayes filter is equivalent to the Kalman filter. By increasing the level of abstraction, the Bayes filter will allow us to have a better understanding of the Kalman filter, and some proofs become easier and more intuitive. As an illustration, we will consider the smoothing problem where the estimation is made more accurate by taking all future measurements, when available. Of course, the smoothing is mainly used for offline applications. 8.2. Basic notions of probabilities Marginal density. If x ∈ Rn and y ∈ Rm are two random vectors with a joint probability density function π(x, y), note that π(x, y) is a function which associates with (˜ x, y ˜) ∈ Rn ×Rm an element of R+ denoted by π(x = x ˜, y = y ˜). The marginal density for x is = π(x) = π(x, y) · dy. [8.1] Note that, to be rigorous, we should have written = π(x = x ˜) = π(x = x ˜, y = y ˜) · d˜ y, y ˜∈Rm

Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

330

Mobile Robotics

but this notation would become too heavy for our applications. In the same manner, the marginal density for y is π(y) =

=

π(x, y) · dx.

The two random vectors x and y are independent if π(x, y) = π(x) · π(y). Conditional density. The conditional density for x given y is π(x|y) =

π(x,y) . π(y)

[8.2]

Again, the quantity π(x|y) is a function which associates with the pair (˜ x, y ˜) ∈ Rn × Rm a positive real number. But, y does not have the same role: it is a parameter of the density of x. We also have π(y|x) =

π(x,y) . π(x)

[8.3]

Bayes rule. Combining the two equations [8.2] and [8.3], we get π(x,y) = π(y|x) · π(x) = π(x|y) · π(y). The Bayes rule obtained from the previous equation is π(x|y) =

π(y|x) · π(x) = η · π(y|x) · π(x). π(y)

[8.4]

1 The quantity η = π(y) , called the normalizer, allows us to have an integral for x equal to one. Sometimes, we use the following notation:

π(x|y) ∝ π(y|x) · π(x) to indicate that the two functions π(x|y) and π(y|x)·π(x) are proportional for a given y. Total probability law. The marginal density for x is = [8.1,8.2] π(x) = π(x|y) · π(y) · dy.

[8.5]

Bayes Filter

331

This corresponds to the law of total probability. Parametric case. If z is a parameter (which can be random vector and any other deterministic quantity), the parametric total probability law is given by = [8.5] π(x|z) = π(x|y,z) · π(y|z) · dy [8.6] and the parametric Bayes rule is [8.4]

π(x|y, z) =

π(y|x,z) · π(x|z) . π(y|z)

[8.7]

Bayes network. A Bayes network is a probabilistic graphical model that represents a set of random vectors and their conditional dependencies. Formally, Bayes networks are directed acyclic graphs, the nodes of which represent random vectors. Arcs represent dependencies. More precisely, the two vectors x, y are connected by an arc and there is a relation that links them. Nodes that are not connected (there is no path from one of the variables to the other in the Bayes network) represent variables that are independent. For better understanding, consider five vectors a, b, c, d, e that are linked by the relations b e c d

= a + α1 = a + 2b + α2 = e + α3 = 3c + α4

where the αi are all independent vectors with a known density, for instance N (0, 1), which means Gaussian-centred with a unit variance. It is clear that if we know a density π(a) for a, we can compute the probabilities for all other vectors. Equivalently, we can easily write a program which generates some realizations for all the vectors. For instance in the scalar case, if π(a) = N (1, 9), the program could be: 1 2 3 4 5

a := 1+randn(9) b := a + randn(1) e := a + 2 · b + randn(1) c := e + randn(1); d := 3 · c + randn(1)

From this program, we easily see that π(c|a, e, b) = π(c|e), which means that given e, the vector c is independent of a and b. The corresponding network is depicted in Figure 8.1 (left). The network will help us to simplify conditional densities. For instance, from the network we can conclude that π(c|a, e, b) = π(c|e), since all

332

Mobile Robotics

paths from a to c and all paths from b to c have to go through e. Equivalently, this means that if we know e the knowledge of a and b does not bring any new information on c. Consider three vectors x, y, z that are linked by the relations y = 2x + α1 z = 3y + α2 x = 4z + α3 where the αi are all independent vectors. The graph (see Figure 8.1 (right)) has a cycle and is not considered as a Bayes network anymore. It is now much more difficult (or even impossible) to compute the marginal densities or to build a program that generates a cloud of points that is consistent with the probabilistic assumptions.

Figure 8.1. Left: a Bayes network. Right: a graph that has a cycle and is thus not a Bayes network

8.3. Bayes filter Consider a system described by the following state equations: ! xk+1 = fk (xk ) + αk yk = gk (xk ) + β k

[8.8]

where αk and β k are random vectors that are white and mutually independent. The dependency with respect to some known inputs uk is taken into account via the functions fk and gk . We have here a random process which satisfies the Markov assumptions, which tells us that the future of the system only depends on the past through the state at the current time t. This can be written as: π(yk |xk , y0:k−1 ) = π(yk |xk ) π(xk+1 |xk , y0:k ) = π(xk+1 |xk ). The notation y0:k has to be understood as follows: y0:k = (y0 , y1 , . . . , yk ) .

[8.9]

Bayes Filter

333

This is illustrated by the Bayes network in Figure 8.2. An arc of this graph corresponds to a dependency. For example, the arc between xk and xk+1 corresponds to the knowledge that xk+1 − fk (xk ) has a density which corresponds to that of αk .

Figure 8.2. A Bayes network associated with the state equation

T HEOREM.– The two densities def

pred (xk ) = π (xk |y0:k−1 )

bel (xk )

[8.10]

def

= π (xk |y0:k )

satisfy (i) pred (xk+1 ) = (ii) bel (xk ) =

B

π (xk+1 |xk ) · π(yk |xk )·pred(xk ) π(yk |y0:k−1 )

bel (xk ) · dxk (prediction) (correction)

[8.11]

N OTE.– These relations correspond to a Bayes observer or Bayes filter. The prediction equation is known as the Chapman–Kolmogorov equation. P ROOF.– Let us prove relation (ii). We have [8.10]

bel (xk ) = π (xk |y0:k ) = π (xk |yk , y0:k−1 ) [8.7]

=

C

1 π(yk |y0:k−1 )

· π(yk |xk , y0:k−1 ) · π(xk |y0:k−1 ) 2 34 5 2 34 5 [8.9]

π(x|y, z) =

= π(yk |x Dk ) π(y|x,z)·π(x|z) π(y|z)

[8.10]

= pred(xk )

Let us now prove (i). From the total probability rule equation [8.6], we get [8.10]

pred (xk+1 ) = π (xk+1 |y0:k ) [8.6] B = π(xk+1 |xk , y0:k ) · π(xk |y0:k ) dxk 2 34 5 2 34 5 [8.9]

[8.10]

= π(x = bel (xk ) B k+1 |xk ) { π(x|z) = π(x|y, z) · π(y|z) · dy }

334

Mobile Robotics

This corresponds to relation (i).! N OTE.– From [8.11, (ii)], we have =

bel (xk ) · dxk = 2 34 5 =1

=

π(yk |xk ) · pred (xk ) · dxk . π (yk |y0:k−1 )

Thus,

π (yk |y0:k−1 ) =

=

π(yk |xk ) · pred (xk ) · dxk

is a normalizer coefficient which can be computed directly from π(yk |xk ) and pred(xk ). The relations [8.11] can be interpreted as an algorithm where the variables pred(xk ) and bel(xk ) are densities, i.e. functions from Rn → R. Such an algorithm cannot be implemented on a computer in the general case, and can only be poorly approximated. In practice, different approaches can be used to implement a Bayes filter. – If the random vector x is discrete, the process can be represented by a Markov chain and the densities pred(xk ) and bel(xk ) can be represented by a vector of Rn . The Bayes filter can be implemented exactly. – If the densities pred(xk ) and bel(xk ) are Gaussian, they can be represented exactly by their expectation and their covariance matrices. We get the Kalman filter [KAL 60]. – We can approximate pred(xk ) and bel(xk ) with a cloud of points (called particles) with a different weight. The corresponding observer is called a particle filter. – We can represent the densities pred(xk ) and bel(xk ) with a subset of Rn which contains the support for the density. We obtain an observer called a set-membership filter. 8.4. Bayes smoother A filter is causal. This means that the estimation x ˆk|k−1 only takes into account the past. The smoothing process consists of a state estimation when all the measurements (future, present and past) are available. Let us denote by N the maximum time k. This time can correspond, for instance, to the end date of a mission performed by the robot, for which mission we are trying to estimate its path. In order to perform smoothing,

Bayes Filter

335

we simply need to run a Kalman filter once more in the backward direction and merge, for each k, the information from the future with that of the past. T HEOREM.– The three densities def

pred (xk ) = π (xk |y0:k−1 )

bel (xk )

def

[8.12]

= π (xk |y0:k )

def

back (xk ) = π (xk |y0:N )

can be defined recursively as follows: B (i) pred (xk ) = π (xk |xk−1 ) · bel (xk−1 ) · dxk−1 (prediction) k |xk )·pred(xk ) (ii) bel (xk ) = π(yπ(y (correction) k |y0:k−1 ) B π(xk+1 |xk )·back(xk+1 ) (iii) back (xk ) = bel (xk ) · dxk+1 (smoother) pred(xk+1 )

[8.13]

P ROOF.– The prediction equation (i) and the correction equation (ii) have already been proven. It remains to prove (iii). We have = [8.6]

=

= (Markov)

=

B

back (xk ) π (xk |y0:N )

π (xk |xk+1 , y0:N ) ·π (xk+1 |y0:N ) · dxk+1 {π(a|c) 2 34 5 || B , π(a|b, c) · π(b|c) · db 52 3 B 4 π (xk |xk+1 , y0:k ) ·back (xk+1 ) · dxk+1 2 34 5 ||

(Bayes)

=

(Markov)

=

=

4 52 3 B π (xk+1 |xk , y0:k ) · π (xk |y0:k ) ·back (xk+1 ) · dxk+1 πC(xk+1 |y0:k ) D π(a|b, c) = π(b|a, c) · π(a|c) π(b|c) B π(xk+1 |xk )·π(xk |y0:k ) · back (x k+1 ) · dxk+1 pred(xk+1 ) B π(xk+1 |xk ) π (xk |y0:k ) pred(xk+1 ) · back (xk+1 ) · dxk+1 2 34 5 bel(xk )

8.5. Kalman smoother

8.5.1. Equations of the Kalman smoother Consider a linear discrete time system described by the state equation. ! xk+1 = Ak xk + uk + αk yk = Ck xk + β k

336

Mobile Robotics

αk and β k are Gaussian, white and independent. An optimized version of the Bayes smoother, in the case where all densities are Gaussian and all functions are linear, is referred to as the Kalman smoother (or Rauch– Tung–Striebel smoother). It can then be applied by adding the smoothing equation of the following theorem to those of the Kalman filter. T HEOREM.– The smoothing density is

where

/ H 0 back (xk ) = π (xk |y0:N ) = N xk Hx ˆk|N , Γk|N , Jk = Γk|k · ATk · Γ−1 / k+1|k 0 x ˆk|N = x ˆk|k + Jk · /x ˆk+1|N − x ˆk+1|k 0 Γk|N = Γk|k − Jk · Γk+1|k − Γk+1|N · JTk .

[8.14]

P ROOF.– See exercise 8.6. 8.5.2. Implementation In order to perform the smoothing process, we first need to run the Kalman filter for k ranging from 0 to N , and then run equations [8.14] backwards for k ranging from N to 0. Note that all the quantities x ˆk+1|k , x ˆk|k , Γk+1|k , Γk|k , x ˆk|N are stored pred up pred up back back xk ,Γk ,Γk ,Γk , where pred, up, back respectively in the lists x ˆk ,ˆ xk ,ˆ mean prediction, update, backward. The quantities u(k), y(k), Γαk , Γβk , Ak are also stored in lists. The direct, or forward, part of the smoother is given by the F ILTER algorithm below. It uses the Kalman function given in section 7.4. pred Function F ILTER (ˆ xpred 0 , Γ0 , u(k), y(k), Γαk , Γβ k , Ak , Ck , k = 0, . . . , N ) 1 For k =- 0 to N . pred up x ˆpred ˆup = k+1 , Γk+1 , x k , Γk . pred 3 K ALMAN x ˆk , Γpred k , u(k), y(k), Γαk , Γβ k , Ak , Ck . pred up up 4 Return x ˆpred , Γ , x ˆ , Γ , k = 0, . . . , N k+1 k+1 k k

2

The backward part of the smoother is given by the following S MOOTHER algorithm.

Bayes Filter

337

up pred Function S MOOTHER (ˆ xpred ˆup k+1 , Γk+1 , x k , Γk , Ak , k = 0, . . . , N )

1 x ˆback ˆup N =x N back 2 ΓN = Γup N 3 For k = N − 1 downto 0 .−1 pred T 4 J = Γup k · Ak · Γk+1 . up pred back 5 x ˆback = x ˆ + J · x ˆ − x ˆ k k k+1 - k+1 . up pred back T 6 Γback = Γ − J · Γ − Γ k k+1 · J k k+1 / back back 0 7 Return x ˆk , Γk , k = 0, . . . , N 8.6. Exercises E XERCISE 8.1.– Conditional and marginal densities

Consider two random variables x, y which belong to the set {1, 2, 3}. The table below gives π(x, y). π(x, y) x = 1 y = 1 0.1 y = 2 0.1 y=3 0

x=2 x=3 0.2 0 0.3 0.1 0.1 0.1

1) Compute the marginal densities π(x) and π(y) for x and y. 2) Compute the conditional densities π(x|y) and π(y|x). E XERCISE 8.2.– Weather forecast Markov chain. A Markov chain is a sequence of discrete random variables x0 , x1 , . . . , xk , . . . , called the state variables, where k is time such that the future of the system only depends on the present state. More precisely, if the current state is known precisely, a full knowledge of the past will not bring any new information to predict the future. We can write the Markov property as π (xk+1 = j| xk = ik , xk−1 = ik−1 , . . . ) = π (xk+1 = j| xk = ik ) . The set of all feasible values for xk is the state space. This probabilistic definition is totally consistent with all the notions of state space used in this book. Markov chains can be described by an oriented graph, the label of which corresponds to the probability of going from one state to another. To each k we associate the vector pk , the ith component of which is defined by pk,i = π(xk = i).

338

Mobile Robotics

Note that all components of pk are positive and their sum is equal to 1. Weather forecast. In the town of Brest, two kinds of weather are possible: sunny coded by 1 and rainy coded by 2. The weather does not change during the day. Denote by xk the weather on day k. We assume that the state xk corresponds to a Markov chain, the conditional density π(xk+1 |xk ) of which is given by: π(xk+1 |xk ) xk = 1 xk = 2 xk+1 = 1 0.9 0.5 xk+1 = 2 0.1 0.5 This can be represented by the following figure.

Figure 8.3. Markov chain describing the evolution of the weather in Brest

1) We represent π(xk ) by a vector pk of dimension 2. Using a Bayes filter, show that the evolution of pk can be represented by a state equation of the form pk+1 = A · pk .

2) At day k, the weather is sunny. What is the probability of it being sunny on day k + *? 3) What about this probability when * tends to infinity? E XERCISE 8.3.– Door robot

In this exercise, we illustrate the Bayes filter in a system with a state x ∈ {0, 1}. It corresponds [THR 05] to a door which can be closed (x = 0) or open (x = 1) and an actuator to open and close. The system also has a sensor to measure the state. The input u can be u = −1, 0 or 1, which mean “close”, “do nothing” or “open”. The required input may fail (for instance, if someone stops the door). We can represent the influence u has on the state evolution with the following table: π(xk+1 |xk , uk ) uk = −1, uk = 0 uk = 1 xk = 0 δ0 δ0 0.2 δ0 + 0.8 δ1 xk = 1 0.8 δ0 + 0.2 δ1 δ1 δ1

Bayes Filter

339

For simplicity, the dependencies of the δi with respect to xk+1 are omitted. When we read in the table π(xk+1 |xk = 0, uk = 1) = 0.2 δ0 + 0.8 δ1 , it means that if xk = 0, there is a probability of 0.8 being at state 1 at time k + 1, if we apply the input uk = 1. The sensor which gives us the state of the door is also uncertain. This is represented by the following conditional density: π(yk |xk ) xk = 0 xk = 1 yk = 0 0.8 0.4 yk = 1 0.2 0.6 The system can be represented by the graph of Figure 8.4.

Figure 8.4. Graph representing the evolution and the observation of the door robot

1) Assuming that the belief at time k = 0 is given by bel(x0 ) = 0.5 · δ0 + 0.5 · δ1 and that u0 = 1, compute pred(x1 ). 2) At time 1, we measure y1 = 1. Compute bel(x1 ). 3) If ppred and pbel k are the stochastic vectors associated with pred(xk ) and bel(xk ), k give the state equation for the Bayes filter.

340

Mobile Robotics

E XERCISE 8.4.– Robot in the forest The objective of this exercise is to give a graphical interpretation of the Bayes filter. We recall that the Bayes filter is given by the following equations: pred (xk+1 ) = bel (xk ) =

B

,

π (xk+1 |xk , uk ) · bel (xk ) · dxk

π(yk |xk )·pred(xk ) π(yk |xk )·pred(xk )·dxk

Figure 8.5 represents a robot with a scalar state x which corresponds to its position on a horizontal line.

Figure 8.5. Illustration of the Bayes filter

Step k. We assume that at time k, we know from the past the prediction pred(xk ), which is uniform over a large interval. The robot knows that in its environment, there are three identical trees. Now, at time k, the robot sees a tree in front of it. This corresponds to the measurement yk . It deduces the likelihood π (yk |xk ) of x. The belief bel(xk ) can thus be obtained by simple multiplication of the density pred(xk ) and the likelihood π (yk |xk ), followed by a normalization.

Bayes Filter

341

Step k+1. The robot moves forward a few meters and the counter k increases by 1. From the belief at time k and the knowledge of the motion, we can predict, via pred(xk+1 ), the state at time k + 1. We can find bel(xk+1 ) in the same way as previously at time k. Step k+2. Now assume that the robot moves forward a few meters again but does not see any trees. Draw pred(xk+2 ), π (yk+2 |xk+2 ) and bel(xk+2 ). E XERCISE 8.5.– Bayes rule with Kalman Consider two Gaussian random variables x ∼ N (1, 1) and b ∼ N (0, 1), i.e. the expectations of x,b are x ¯ = 1, ¯b = 0 and the variances (i.e. the covariance matrix in the scalar case) of x,b are σx2 = 1, σb2 = 1. 1) Define y = x + b. Give the expression of the probabilility density function of y. 2) We collect the following measurement y = 3. Using the correction equations of the Kalman filter, compute the posterior density for x. 3) Provide the link with the Bayes rule. E XERCISE 8.6.– Derivation of the Kalman smoother 1) Consider two normal vectors a and b. Show that Q π(a) = N (a -ˆ a, Γa ) π(b|a) = N (b H + u, R ) / -Ja 0 =⇒ π(b) = N b HJˆ a + u, R + J · Γa · JT .

[8.15]

2) Consider a linear discrete-time system described by the state equation. !

xk+1 = Ak xk + uk + αk yk = Ck xk + β k

where αk and β k are Gaussian, white and independent. Using the equations of the Kalman filter, show that / H / 0 π (xk |xk+1 , y0:N ) = N xk Hx ˆk|k + Jk xk+1 − x ˆk+1|k , 0 [8.16] Γk|k − Jk · Γk+1|k · JTk

where

Jk = Γk|k · ATk · Γ−1 k+1|k .

342

Mobile Robotics

3) From the two previous questions, show that the smoothing density is

where

/ H 0 back (xk ) = π (xk |y0:N ) = N xk Hx ˆk|N , Γk|N / 0 x ˆk|N = x ˆk|k + Jk · /x ˆk+1|N − x ˆk+1|k 0 Γk|N = Γk|k − Jk · Γk+1|k − Γk+1|N · JTk .

E XERCISE 8.7.– SLAM

The Redermor (underwater robot built by GESMA, Brest) performed a two-hour mission in the Douarnenez bay (see Figure 8.6). During its mission, it collected data from its inertial unit (which gives us the Euler angles φ, θ, ψ), its Doppler log (which gives us the robot’s speed vr in the robot’s coordinate system), its pressure sensor (which gives us the robot’s depth pz ) and its altitude sensor (a sonar that gives us the altitude a), with a sampling period of dt = 0.1 sec. This data can be found in the file slam_data.txt1. The file is composed of 59,996 lines (one line per sampling period) and nine columns which are respectively: (t, ϕ, θ, ψ, vx , vy , vz , pz , a) where pz is the depth of the robot and a is its altitude (i.e. its distance from the seabed).

Figure 8.6. The Redermor, built by GESMA (Groupe d’Etude Sous-Marine de l’Atlantique), right before diving into the water 1 Available at: https://www.ensta-bretagne.fr/jaulin/isterob.html.

Bayes Filter

343

1) Given that the robot started from a position p = (0, 0, 0) at time t = 0, and using a Euler method, deduce an estimation for the path. Use the state equation: p(t) ˙ = R(ϕ (t) , θ (t) , ψ (t)) · vr (t) with R(ϕ, θ, ψ) the Euler matrix (see equation [1.9]):     cos ψ − sin ψ 0 cos θ 0 sin θ 1 0 0 1 0   0 cos ϕ − sin ϕ  R(ϕ, θ, ψ) =  sin ψ cos ψ 0   0 0 0 1 − sin θ 0 cos θ 0 sin ϕ cos ϕ

The angles ϕ, θ, ψ 0are measured with a standard deviation of / 2) −4 2 × 10 , 2 × 10−4 , 5 × 10−3 . The components of vr are measured every dt seconds with a standard deviation of σv = 1 ms−1 . We may assume that the robot satisfies the equation: pk+1 = pk + (dt · R(k)) · v ¯r (k) + αk where αk is a white noise and v ¯r (k) is a measurement of the average speed over the corresponding sampling period. Show that a realistic covariance matrix for αk is   1 0 0 Γα = dt2 σv2 ·  0 1 0  . 0 0 1

N OTE.– We are not in the situation described in section 7.5 or exercise 7.6 where the 1 covariance for the state noise should have the same order as dt . Here, our system is not a discretisation of a continuous random process, the behavior of which should be independent of dt. On the contrary, dt is a sampling time of the velocity sensor. The smaller dt is, the more accurate the integration will be. 3) Using the Kalman filter as a predictor, calculate the precision with which the robot knows its position at each moment t = k · dt. Give, as a function of t, the standard deviation of the error over the position. What will this become after an hour? After two hours? Verify your calculations experimentally by implementing a Kalman predictor. 4) During its mission, the robot may detect several landmarks with its lateral sonar (here, these will be mines). When the robot detects a landmark, it will be on its right side and in a plane perpendicular to the robot. The seabed is assumed to be flat and horizontal. The following table shows the detection times, the numbers i of the landmarks and the distance ri between the robot and the landmark: t 1054 1092 1374 1748 3038 3688 4024 4817 5172 5232 5279 5688 i 1 2 1 0 1 5 4 3 3 4 5 1 ri (t) 52.4 12.47 54.4 52.7 27.73 27.0 37.9 36.7 37.37 31.03 33.5 15.05

344

Mobile Robotics

SLAM (Simultaneous Localization And Mapping) seeks to use these repeated detections to improve the precision of the estimation of its path. For this, we form a large state vector x, of dimension 3 + 2 × 6 = 15, which contains the position of the robot p as well as the vector q of dimension 12 containing the coordinates (as x and y) of the six landmarks. Let us note that since the landmarks are immobile, we have q˙ = 0. Give the function (y, Ck , Γβ ) = g (k) that corresponds to the observation. This function returns the measurement vector y, the matrix C(k) and the covariance matrix of the measurement noise. As for the standard deviation of the measurement noise βk , we will take 0.1 for that of the depth and 1 for that of the robot–landmark distance. 5) Using a Kalman filter, find the position of the landmarks together with the associated uncertainty. Show how the robot was able to readjust its position. 6) Use the Kalman smoother to improve the precision of the landmark positions by taking into account the past as well as the future. E XERCISE 8.8.– a priori SLAM An underwater robot carries a navigation system (inertial unit and Doppler log) that gives its position as x, y with a drift of 100 meters per hour. This means that if the robot knows its position with a precision of r meters at time t, then an hour before and an hour later it knows its position with an error smaller than r + 100 m. At the beginning, our robot locates itself by GPS with a precision of 10 m and proceeds to dive into a zone of flat seabed. It swims around for eight hours at constant depth (which it can measure with a pressure sensor). When it resurfaces, it locates itself again using the GPS, again with a precision of 10 m. Each hour, the robot passes over a remarkable landmark (a small rock with a particular form, for instance) that can be detected by a camera fitted under the robot. The following table indicates the number of the detected landmark as a function of time, expressed in hours: t(hour) 1 2 3 4 5 6 7 landmark 1 2 1 3 2 1 4 We may deduce from this table that the robot encounters four remarkable landmarks in total and that it encounters landmark 1 three times, at times t = 1H, t = 3H and t = 6H. With what precision is it able to localize landmarks 1, 2, 3 and 4?

Bayes Filter

345

8.7. Corrections C ORRECTION FOR E XERCISE 8.1.– (Conditional and marginal densities) 1) The marginal density for x is [8.1] B π(x) = Pπ(x, y) · dy = j∈{1,2,3} π(x, y = j) = π(x, y = 1) + π(x, y = 2) 2 34 5 2 34 5 0.1δ1 (x)+0.2δ2 (x)

+

0.1δ1 (x)+0.3δ2 (x)+0.1δ3 (x)

= 0.2 δ1 (x) + 0.6 δ2 (x) + 0.2 δ3 (x)

π(x, y = 3) 2 34 5

0.1δ2 (x)+0.1δ3 (x)

where δi (x) is the function which is 1 for x = i and 0 if x &= i. The marginal density for y is π(y) =

=

π(x, y) · dx = 0.3 δ1 (y) + 0.5 δ2 (y) + 0.2 δ3 (y).

2) The conditional density for y given x is [8.3]

π(y|x) =

π(x, y) = π(x)

1

1

i∈{1,2,3} j∈{1,2,3}

π(x = i, y = j) · δi (x) · δj (y) π(x = i)

1 1 1 δ1 (x)δ1 (y) + δ1 (x)δ2 (y) + δ2 (x)δ1 (y) 2 2 3 1 1 1 + δ2 (x)δ2 (y) + δ2 (x)δ3 (y) + δ3 (x)δ2 (y) 2 6 2 1 + δ3 (x)δ3 (y). 2 =

It can be represented by the following table: π(y|x) x = 1 x = 2 x = 3 1 1 y=1 0 2 3 1 1 1 y=2 2 2 2 1 1 y=3 0 6 2 In the same manner, we can compute π(x|y) from π(x, y): π(x|y) x = 1 x = 2 x = 3 1 2 y=1 0 3 3 1 3 1 y=2 5 5 5 1 1 y=3 0 2 2

346

Mobile Robotics

This result could also have been obtained directly from π(y|x), using the Bayes rule. C ORRECTION FOR E XERCISE 8.2.– (Weather forecast) In the case where we have no input u and no output y, the Bayes filter is B bel (xk+1 ) = Pπ (xk+1 |xk ) · bel (xk ) · dxk = i∈{1,2} π (xk+1 |xk = i) · bel (xk = i) . Therefore,

π (xk+1 = j) = 2 34 5 pk+1,j

1

i∈{1,2}

or equivalently

π (xk+1 = j|xk = i) · π (xk = i) 2 34 5 2 34 5 aij

pk,i

pk+1 = A · pk , with A=

"

0.9 0.5 0.1 0.5

#

.

Note that the sum of all entries of each column is equal to 1. The quantity aij corresponds to the probability that the weather tomorrow will be j given that the time today is i. 2) If day k is sunny then * days later, the probability of it being sunny is given by the first component of the vector pk+) =

"

0.9 0.5 0.1 0.5

#) " # 1 . 0

3) The matrix A has the eigenvalues λ1 = 0.4 and λ2 = 1. The eigenvalue λ1 = 0.4 is stable whereas λ2 = 1 corresponds to stationary behavior. This is due to the fact that pk is stochastic (i.e. the sum of its components is equal to 1). The stochastic eigenvector associated with λ2 is (0.833, 0.167). Thus, for any initial weather, after a sufficiently long time, the probability of a sunny day is 0.833. C ORRECTION FOR E XERCISE 8.3.– (Door robot) 1) We have B pred (xk+1 ) = π (xk+1 |xk , uk ) · bel (xk ) · dxk = π (xk+1 |xk = 0, uk ) · bel (xk = 0) +π (xk+1 |xk = 1, uk ) · bel (xk = 1).

Bayes Filter

If u0 = 1 and bel (x0 ) = 0.5 · δ0 + 0.5 · δ1 , we get pred (x1 ) = π (x1 |x0 = 0, u0 = 1) · bel (x0 = 0) 2 34 5 2 34 5 0.5

0.2 δ0 +0.8 δ1

+ π (x1 |x0 = 1, u0 = 1) · bel (x0 = 1) 2 34 5 2 34 5 0.5

0 δ0 +1 δ1

= 0.1 δ0 (x1 ) + 0.9 δ1 (x1 )

2) If y1 = 1, we have [8.11]

π(y1 =1|x1 )·pred(x1 ) bel (x1 ) = , π(y 1 =1|x1 )·pred(x1 )·dx1 ∝ π(y1 = 1|x1 ) · pred (x1 ) = (0.2 δ0 (x1 ) + 0.6 δ1 (x1 )) · (0.1 δ0 (x1 ) + 0.9 δ1 (x1 )).

Thus, bel (x1 ) =

0.02 · δ0 (x1 ) + 0.54 · δ1 (x1 ) . 0.56

3) For the prediction, we have ppred k+1

"

# " # 1 0.8 1 0 bel = δ−1 (uk ) · · pk + δ0 (uk ) · · pbel k 0 1 " 0 0.2 # 0.2 0 +δ1 (uk ) · · pbel k . 0.8 1

For the belief, we have [8.11]

bel (xk ) ∝ (δ0 (yk ) · π(yk = 0|xk ) · pred (xk ) +δ1 (yk ) · π(yk = 1|xk ) · pred (xk )). Or equivalently, pbel k ∝

" 2

δ0 (yk ) ·

"

0.8 0 0 0.4

#

· ppred + δ1 (yk ) · k 34 def

= qk

"

0.2 0 0 0.6

#

· ppred k

#

5

347

348

Mobile Robotics

and thus pbel k =

qk . qk,1 + qk,2

As a consequence, the Bayes filter is " " # " # 1 0.8 10 ppred = δ (u ) · + δ (u ) · −1 k 0 k k+1 0 0.2 01 " ## 0.2 0 +δ1 (uk ) · · pbel k 0.8 1 " " # " ## 0.8 0 0.2 0 qk = δ0 (yk ) · + δ1 (yk ) · · ppred k 0 0.4 0 0.6 pbel k =

qk . qk,1 + qk,2

The filter has uk and yk as inputs. Its output is the belief pbel k . C ORRECTION FOR E XERCISE 8.4.– (Robot in the forest) The quantities pred(xk+2 ), π (yk+2 |xk+2 ) and bel(xk+2 ) are depicted in Figure 8.7. The likelihood π (yk+2 |xk+2 ) is maximal except in the part of the state space where the robot has a high probability of seeing a tree. Thus, the robot uses the information of the non-visibility of a tree to improve its localization.

Figure 8.7. The robot uses the information of the non-visibility of a tree to improve its localization

C ORRECTION FOR E XERCISE 8.5.– (Bayes rule with Kalman) 1) We have y¯ = 1 and σy2 = σx2 + σb2 = 2. Therefore, " # 1 1 2 πy (y) = √ · exp − (y − 1) . 4 4π

Bayes Filter

349

2) We define the random variable v = x|y. Recall that the correction equations of the Kalman filter are (i) (ii) (iii) (iv) (v)

x ˆk|k Γk|k @k y Sk Kk

= = = = =

@k x ˆk|k−1 +Kk · y Γk|k−1 − Kk · Ck Γk|k−1 yk − C k x ˆk|k−1 Ck Γk|k−1 CTk + Γβk Γk|k−1 CTk S−1 k

In our context, we get: (i) (ii) (iii) (iv) (v)

v¯ σv2 y@k Sk Kk

= = = = =

x ¯ + Kk · y@k = 1 + 12 .2 = 2 σx2 − Kk · σx2 = 1 − 12 = 12 y−x ¯=3−1=2 σx2 + σb2 = 1 + 1 = 2 σx2 Sk−1 = 12

Therefore, v¯ = 2 and σv2 = 12 . Thus, " # 1 πv (v) = < · exp − (v − 2) · 2 · (v − 2) 2 (2π) det( 12 ) 1

. 1 2 = √ · exp − (v − 2) . π

3) We have v = (x|y) and the correction made by the Kalman filter implements the Bayes rule π(x|y) =

π(y|x) · π(x) π(y)

in the Gaussian case. C ORRECTION FOR E XERCISE 8.6.– (Derivation of the Kalman smoother) 1) Take b = Ja + u + r, where r ∼ N (0, R). We have π(b|a) = N (b -Ja + u, R ), as in the assumption. Now, from the Bayes rule, we know that from π(b|a) and π(a), there exists a unique way π(b). We#get "- to write .T . ˆ = Jˆ ˆ ˆ E(b) = b a + u. The covariance matrix for b is E b−b b−b = . ˆ) + r)T (J (a − a ˆ) + r) = R + J · Γa · JT . E (J (a − a 2) From the Markov property, we have

π (xk |xk+1 , y0:N ) = π (xk |xk+1 , y0:k )

350

Mobile Robotics

Let us now introduce a virtual step, as represented in the Bayesian network of Figure 8.8, and let us apply one step of the Kalman filter to get π(xk |xk+1 , y0:k ). The quantity xk+1 − uk plays the role of the measurement.

Figure 8.8. A virtual step is added to allow us to use the Kalman equations

This expression is obtained directly from the Kalman filter. In this case, the innovation is xk+1 − x ˆk+1|k , and the Kalman gain corresponds to Jk . The correspondence with the correction equations of the Kalman filter (see equation [7.11]), or equivalently with the linear estimator (see equation [7.10]), is the following:   @k x ˆk|xk+1 ,y0:k = x ˆk|k +Jk · @ zk x ˆk|k = x ˆk|k−1 +Kk · y         Γ = (I − J A Γ = (I − K C ) Γ  k|xk+1 ,y0:k  k|k k k ) Γk|k k k k|k−1 @ k = yk − Ck x @ y ˆk|k−1 zk = (xk+1 − uk ) − Ak x ˆk|k −→   T T   S = C Γ C + Γ S = A Γ A + Γ   k k k|k−1 k β k k k|k k α     Kk = Γk|k−1 CTk S−1 Jk = Γk|k ATk S−1 k k Now, from the prediction equations of the Kalman filter, given by ! x ˆk+1|k = Ak x ˆk|k + uk (i) Γk+1|k = Ak · Γk|k · ATk + Γα (ii)

we have  + , (i) + ,  ˆk|xk+1 ,y0:k = x ˆk|k + Jk · xk+1 − uk − Ak x ˆk|k = x ˆk|k + Jk xk+1 − x ˆk+1|k x Γk|xk+1 ,y0:k = Γk|k − Jk · Ak Γk|k

 J

(ii)

= Γk|k · ATk · Γ−1 k+1|k

k

= Γk|k − Jk · Γk+1|k · JTk

3) Since H 0 / π (xk+1 |y0:N ) = N xk+1 Hx ˆk+1|N , Γk+1|N H / 0 [8.16] ˆk|k − Jk x ˆk+1|k , π (xk |xk+1 , y0:N ) = N (xk HJk xk+1 + x Γk|k − Jk · Γk+1|k · JTk )

Bayes Filter

351

then, by taking y0:N as a parameter and with the correspondences a ˆ a Γa b R J u

−→ xk+1 −→ x ˆk+1|N −→ Γk+1|N −→ xk −→ Γk|k − Jk · Γk+1|k · JTk −→ Jk −→ x ˆk|k − Jk x ˆk+1|k

equation [8.15] yields π (xk |y0:N )  H  H H / 0 / 0  H  = N xk H x ˆ + Jk x ˆk+1|N − x ˆk+1|k , Γk|k − Jk · Γk+1|k − Γk+1|N · JTk  . H2 k|k 34 5 2 34 5 H Γ x ˆ k|N

k|N

C ORRECTION FOR E XERCISE 8.7.– (SLAM)

1) The simulation program using Euler’s method is the following: -− − → − → − → − →. → − t ,→ ϕ, θ , ψ,→ v r, d , − a :=readfile(’slam_data.txt’)   0 − → dt := 0.1; N := size( t ); x ˆ :=  0  0 For k = 1 : N . → − → − → − u := dt · R → ϕ (k), θ (k), ψ (k) · − v (k) r

x ˆ :=ˆ x+u

2) The Euler angles are well known, given the inertial unit. We therefore have: pk+1 = pk + dt · R(ϕk , θk , ψk ) · (¯ vr (k) + αv (k)) = pk + dt · R(ϕk , θk , ψk ) · v ¯r (k) + dt · R(ϕk , θk , ψk ) · αv (k) 2 34 5 2 34 5 uk

=αk

We can approximate αk with a white noise of covariance matrix

Γα = dt2 · R(ϕk , θk , ψk )Γαv RT (ϕk , θk , ψk ) = dt2 σv2 · I since RRT = I −2 = 10 · I.

352

Mobile Robotics

Note that we have not taken into account the errors due to discretization, nor the errors on the angles of the unit. 3) When the Kalman filter is used in predictor mode, we have Γk+1|k = Ak · Γk|k · ATk + Γαk = Ak · Γk|k−1 · ATk + Γα As no measurements are available in predictor mode, we have Γk|k = Γk|k−1 = Γk and thus the equation becomes Γk+1 = Ak · Γk · ATk + Γα . Therefore, Γ1 = A0 · Γ0 · AT0 + Γα

Γ2 = A1 · Γ1 · AT1 + Γα = A1 A0 · Γ0 · AT0 AT1 + A1 Γα AT1 + Γα

Γ3 = A2 · Γ2 · AT2 + Γα = A2 A1 A0 · Γ0 · AT0 AT1 AT2 + A2 A1 Γα AT1 AT2 + A2 Γα AT2 +Γα

However, in this context the matrices Ai are equal to the identity matrix. We therefore have Γk = k · Γα = kdt2 σv2 I Note that this means that the covariance matrix increases linearly with time k. The √ standard deviation then increases by k, which is a known phenomenon in random walk theory. Since t = k · dt, the covariance of the predicted state is Γx (t) =

t t · Γα = dt2 σv2 I = t · dt · σv2 · I dt dt

which corresponds to a standard deviation (or drift) of: √ √ σx (t) = σv t · dt = 0.3 t After one hour, the error is equal √ to σx (3600) = 0.3 · two hours, σx (2 · 3600) = 0.3 · 2 · 3600 = 25 m. The code corresponding to the predictor is given below:

√

3600 = 18 m and after

Bayes Filter

353

-→ → − − → → − → →. − → t ,− ϕ, θ , ψ,− v r, d , − a :=readfile(’slam_data.txt’) → − dt := 0.1; N := size( t ); 0 x ˆ :=  0  ; Γx := 03×3 ; Γα := 10−2 · I3×3 ; A := I3×3 ; y := ∅;Γβ := ∅;C := ∅ 0 For k := 1 : N − → − → . − → u := dt · R − ϕ (k), θ (k), ψ (k) · → v (k) r

(ˆ x,Γx ) :=K ALMAN (ˆ x, Γx , u, y, Γα , Γβ , A, C)

Let us note that since we have no measurements, the quantities y, C, Γβ are empty. The results of the predictor are illustrated in Figure 8.9.

Figure 8.9. Confidence tube obtained with the predictor

4) We will denote by np the space dimension (here equal to 3), by nm the number of landmarks and by nx the dimension of the state vector. The observation equation is given by " # 0 pk y1 = 0 0 1 0 0 0 0 0 0 0 0 0 . . . 0 0 + βk 2 34 5 2 34 5 qk 2 34 5 yk C(k) /

0

/

xk

354

Mobile Robotics

if no landmark is detected. It is given by     # y1 1 0 0 0 0 . . . 0 0 −1 0 0 0 . . . 0 0 "  y2  =  0 1 0 0 0 . . . 0 0 0 −1 0 0 . . . 0 0  pk + β k q y3 0 0 1 0 0 . . . 0 0 0 0 0 0 . . . 0 0 2 34k 5 2 34 5 2 34 5 xk yk

C(k)

in the case where the ith landmark mi is detected. In this second case, the sub-vector (y1 , y2 ) represents the first two components of the vector: 

 0 : p − mi = R(k) ·  − ri2 (k) − a2 (k)  . −a(k)

The y3 component corresponds to the depth measurement given by the pressure sensor. The evolution function is the following: Function g(k) / 0 − → 1 y := d(k); C := 0 0 1 0 0 0 0 . . . 0 ; Γβ := 0.01 10540 10920 13740 . . . 56880 2 1 ... 1  3 T :=  1 52.42 12.47 54.40 . . . 15.05 8 For j := 1 : 2n m k0  m = T(:, j) r if k0 = k   0 . : − → − → → → e := R − ϕ (k), θ (k), ψ (k) ·  − r2 − − a 2 (k)  − → − a (k)     e1 1 0 0 0 0 . . . 0 0 −1 0 0 0 . . . 0 0 y :=  e2 ; C = 0 1 0 0 0 . . . 0 0 0 −1 0 0 . . . 0 0  y 0 0 1 0 0 ... 0 0 0 0 0 0 ... 0 0 Γβ := 10−2 · I3×3 10 Return (y, Ck , Γβ ) Note that the dimension of the outputs depends on the detection (or non-detection) of a landmark. 5) The evolution of the robot–landmark system can be described by: " # " #" # " # pk+1 I3 0 dt · R(k) · vr pk = + + αk . qk+1 0 I12 qk 012×1 2 34 5 2 34 52 34 5 2 34 5 xk+1

A

xk

uk

Bayes Filter

355

We use lists to memorize all the intermediate results so that we are able to proceed with the smoothing (next question). The associated confidence tube is drawn in Figure 8.10.

Figure 8.10. Confidence tube obtained with the Kalman filter together with the ellipses for the six landmarks

The script corresponding to the Kalman filter is: -− − → − → → → − −. → − t ,→ ϕ, θ , ψ,− v r, d , → a :=readfile(’slam_data.txt’) np := 3; nm := 6; nx := np + 2nm − → dt := 0.1; N := size( t ) / 0 pred pred x ˆ1 := 0nx/; Γ1 := diag I30, 106 .I2nm Γαk :=diag 0.01 · Inp , 02nm For k := 1 : N > . ? − → → − → → R − ϕ (k), θ (k), ψ (k) · − v r (k) A := Inx ; u(k) :=dt · 02nm / 0 y(k), C , Γ := g(k) k βk . pred up up pred x ˆpred , Γ , x ˆ , Γ := K ALMAN x ˆpred k+1 k+1 k , Γk , u(k), y(k), 0 k k Γαk , Γβk , Ak , Ck

356

Mobile Robotics

6) We append the following statements to the Kalman filter: up back x ˆback ˆup N := x N ; ΓN := ΓN For k = N − 1 downto 0 .−1 pred T J := Γup k · Ak · Γk+1 . up pred back x ˆback := x ˆ + J · x ˆ − x ˆ k k k+1 - k+1 . up pred back T Γk := Γk − J · Γk+1 − Γback k+1 · J

The associated tube is illustrated in Figure 8.11. Note that the confidence ellipses only get smaller when using the filter, especially near the end of the mission.

Figure 8.11. Confidence tube obtained with the Kalman smoother together with the ellipses for the six landmarks

C ORRECTION FOR E XERCISE 8.8.– (a priori SLAM) The reasoning is given in the table below: t(H) Landmark (a) (b) (c) (d) (e)

0 0 10 10 10 10 10

1 1 110 710 110 110 110

2 2 210 610 210 210 210

3 1 310 510 310 110 110

4 3 410 410 410 310 210

5 2 510 310 310 210 210

6 1 610 210 210 110 110

7 4 710 110 110 110 110

8 0 810 10 10 10 10

Bayes Filter

357

Line (a): precision obtained by propagation in the direction of time. Line (b): precision obtained by back-propagation in the opposite direction of time. Line (c): minimum of lines (a) and (b). Line (d): correspondence between the landmarks. Line (e): propagation in the direct and opposite directions of time.

References

[BEA 12] B EARD R., M C L AIN T., Small Unmanned Aircraft, Theory and Practice, Princeton University Press, 2012. [BOY 06] B OYER F., A LAMIR M., C HABLAT D. et al., “Robot anguille sous-marin en 3D”, Techniques de l’Ingénieur, 2006. [CHE 08] C HEVALLEREAU C., B ESSONNET G., A BBA G. et al. (eds), Bipedal Robots: Modeling, Design and Walking Synthesis, ISTE Ltd, London and John Wiley & Sons, New York, 2008. [COR 11] C ORKE P., Robotics, Vision and Control, Springer, Berlin, Heidelberg, 2011. [CRE 14] C REUZE V., “Robots marins et sous-marins ; perception, modélisation, commande”, Techniques de l’Ingénieur, 2014. [DEL 93] DE L ARMINAT P., Automatique, Commande des Systèmes Linéaires, Hermès, Paris, France, 1993. [DRE 11] D REVELLE V., Etude de Méthodes Ensemblistes Robustes Pour une Localisation Multisensorielle Intègre, Application à la Navigation des Véhicules en Milieu Urbain, PhD dissertation, Université de Technologie de Compiègne, Compiègne, France, 2011. [DUB 57] D UBINS L.E., “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents”, American Journal of Mathematics, vol. 79, no. 3, pp. 497–516, 1957. [FAN 01] FANTONI I., L OZANO R., Non-linear Control for Underactuated Mechanical Systems, Springer-Verlag, 2001. [FAR 08] FARRELL J., Aided Navigation: GPS with High Rate Sensors, McGraw-Hill, 2008. [FLI 13] F LIESS M., J OIN C., “Model-free control”, International Journal of Control, vol. 86, no. 12, pp. 2228–2252, 2013. [FOS 02] F OSSEN T., Marine Control Systems: Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles, Marine Cybernetics, 2002. [GOR 11] G ORGUES T., M ÉNAGE O., T ERRE T. et al., “An innovative approach of the surface layer sampling”, Journal des Sciences Halieutique et Aquatique, vol. 4, pp. 105–109, 2011.

Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

360

Mobile Robotics

[HER 10] H ERRERO P., JAULIN L., V EHI J. et al., “Guaranteed set-point computation with application to the control of a sailboat”, International Journal of Control Automation and Systems, vol. 8, no. 1, pp. 1–7, 2010. [JAU 05] JAULIN L., Représentation D’état Pour la Modélisation et la Commande des Systèmes (Coll. Automatique de base), Hermès, London, 2005. [JAU 10] JAULIN L., “Commande d’un skate-car par biomimétisme”, CIFA 2010, Nancy, France, 2010. [JAU 12] JAULIN L., L E BARS F., “An interval approach for stability analysis; application to sailboat robotics”, IEEE Transaction on Robotics, vol. 27, no. 5, 2012. [JAU 15] JAULIN L., Automation for Robotics, ISTE Ltd, London and John Wiley & Sons, New York, 2015. [JAU 02] JAULIN L., K IEFFER M., WALTER E. et al., “Guaranteed robust nonlinear estimation with application to robot localization”, IEEE Transactions on Systems, Man and Cybernetics: Part C Applications and Reviews, vol. 32, 2002. [KAI 80] K AILATH T., Linear Systems, Prentice Hall, Englewood Cliffs, 1980. [KAL 60] K ALMAN E.R., “Contributions to the theory of optimal control”, Bol. Soc. Mat. Mex., vol. 5, pp. 102–119, 1960. [KHA 02] K HALIL H.K., Nonlinear Systems, Third Edition, Prentice Hall, 2002. [KLE 06] K LEIN E.M.V., Aircraft System Identification: Theory And Practice, American Institute of Aeronautics and Astronautics, 2006. [LAR 03] L AROCHE B., M ARTIN P., P ETIT N., Commande par Platitude, Equations Différentielles Ordinaires et aux Dérivées Partielles, Available at: http://cas.ensmp.fr/∼petit/ensta/main.pdf, 2003. [LAT 91] L ATOMBE J., Robot Motion Planning, Kluwer Academic Publishers, Boston, 1991. [LAU 01] L AUMOND J., La Robotique Mobile, Hermès, Paris, France, 2001. [LAV 06] L AVALLE S., Planning Algorithm, Cambridge University Press, 2006. [MUR 89] M URATA T., “Petri Nets: Properties, Analysis and Applications”, Proceedings of the IEEE, vol. 77, no. 4, 1989. [ROM 12] ROMERO -R AMIREZ M., Contribution à la commande de voiliers robotisés, PhD dissertation, Université Pierre et Marie Curie, France, 2012. [SLO 91] S LOTINE J.J., L I W., Applied Nonlinear Control, Prentice Hall, 1991. [SPO 01] S PONG M., C ORKE P., L OZANO R., “Nonlinear control of the Reaction Wheel Pendulum”, Automatica, vol. 37, pp. 1845–1851, 2001. [THR 05] T HRUN S., B UGARD W., F OX D., Probabilistic Robotics, MIT Press, Cambridge, 2005. [WAL 14] WALTER E., Numerical Methods and Optimization; a Consumer Guide, Springer, London, 2014.

Index

A accelerometer, 222 adjoint matrix, 20 adjoint of a rotation vector, 6 anchoring, 157 angle, 147 artificial potential field, 191 atan2, 12, 233 AUV, 30 B backstepping, 163 bank, 11 barometer, 224 Bayes filter, 329, 333 Bayes observer, 333 Bayes rule, 330 bearing, 225 Bernstein polynomials, 188 Bézier polynomials, 188 biased estimator, 269 biomimetics, 133, 139 Brownian noise, 286 C Cardan angles, 11 cart, 75 Chapman–Kolmogorov, 333 confidence ellipse, 267 correction, 275

covariance, 264 matrix, 264 crank, 95 cross product, 5 D dead reckoning, 11, 293, 317 Delaunay triangulation, 189 Denavit–Hartenberg parametrization, 31 derivative of a quadratic form, 244 DGPS, 224 differential delay, 72 graph, 83 differential dependency graph, 102, 116 direction cosine matrix, 9 Doppler log, 222 Dubins car, 195 Dubins path, 196 duty cycle, 143 dynamic feedback linearization, 119 dynamic model, 90 E, F elevation, 11 error dynamics equation, 66 estimator, 247 Euler angles, 11 Euler’s second law, 18 extended Kalman filter, 283 feedback linearization, 65, 69 filtering, 263

Mobile Robotics, First Edition. Luc Jaulin. © ISTE Ltd 2019. Published by ISTE Ltd and John Wiley & Sons, Inc.

362

Mobile Robotics

flying drone, 161 Fortune’s algorithm, 208 G generalized inverse, 247 geographical coordinates, 183 GESMA, 28 GNSS, 224 goniometric localization, 225 GPS, 224, 230 guidance, 183 gyroscope (gyro), 222 H, I, J heading, 11 high-gain controller, 90 high-gain proportional controller, 93 homogeneous coordinates, 29 hovercraft, 102 identifiability, 259 identification, 243 independence, 264 inertial unit, 16, 223 inscribed angles, 226 inscribed arc, 227 Jacobi identity, 20 K, L Kalman filter, 275, 334 Kalman gain, 271 Kalman smoother, 334 Kalman–Bucy, 280 kinematic GPS, 224 kinematic model, 17, 90, 93 least squares, 246 lidar, 232 linear estimator, 269 linear with respect to the parameters, 246 linearizing feedback, 69 local map, 184 localization, 221, 252 M, N manipulator robot, 31 marginal density, 329 Markov chain, 337

mimetic control, 134 model-free control, 134 modeling, 1 modulus, 135 multilateration, 230 navigation, 221 nonlinear control, 65 O, P odometer, 221 operational amplifiers, 91 orthogonal estimator, 269 particle filter, 334 path planning, 187 PID, 67, 71 pitch, 11 point cloud, 264 polar curve, 83 prediction, 276 preudoinverse, 71 probability density function, 329 profiling float, 104 proportional-derivative controller, 66, 79 PWM, 143 Q, R quadratic form, 243, 244 quadratic function, 243 quadrotor, 18 random walk, 305, 352 Rauch–Tung–Striebel Smoother, 336 Redermor, 28 redundant, 71 relative degree, 71 residual, 247 Rodrigues’ formula, 21 roll, 11 rotation matrix, 2 rotation vector, 5 ROV, 136 Runge Kutta, 26 Runge’s phenomenon, 188 S sailboat, 82, 145 SAUCISSE, 101

Index

sawtooth, 150, 167, 174, 208, 213 function, 134 scale factor, 291 Schuler period, 26 set of acceptable outputs, 73 set-membership filter, 334 simple pendulum, 68 simulated annealing, 250, 261 singularities, 73 skating robot (skate car), 137, 142 sliding mode, 87, 89 smoothing, 334 SNAME, 13 special orthogonal group, 3 state representation, 1 state space, 337 static controller, 80, 86

Staubli, 31 surface, 89 sweep line algorithm, 208 T, V tanh, 134 telemeter, 252 triangulation, 228 variation, 264 Varignon’s formula, 21 Voronoi diagram, 189 W, Y, Z wheel, 31 whiteness, 266 yaw, 11 zero dynamics, 94

363

Other titles from

in Systems and Industrial Engineering – Robotics

2019 ANDRÉ Jean-Claude Industry 4.0: Paradoxes and Conflicts BRIFFAUT Jean-Pierre From Complexity in the Natural Sciences to Complexity in Operations Management Systems (Systems of Systems Complexity Set – Volume 1) BUDINGER Marc, HAZYUK Ion, COÏC Clément Multi-Physics Modeling of Technological Systems FLAUS Jean-Marie Cybersecurity of Industrial Systems KUMAR Kaushik, DAVIM Paulo J. Optimization for Engineering Problems VANDERHAEGEN Frédéric, MAAOUI Choubeila, SALLAK Mohamed, BERDJAG Denis Automation Challenges of Socio-technical Systems

2018 BERRAH Lamia, CLIVILLÉ Vincent, FOULLOY Laurent Industrial Objectives and Industrial Performance: Concepts and Fuzzy Handling GONZALEZ-FELIU Jesus Sustainable Urban Logistics: Planning and Evaluation GROUS Ammar Applied Mechanical Design LEROY Alain Production Availability and Reliability: Use in the Oil and Gas Industry MARÉ Jean-Charles Aerospace Actuators 3: European Commercial Aircraft and Tiltrotor Aircraft MAXA Jean-Aimé, BEN MAHMOUD Mohamed Slim, LARRIEU Nicolas Model-driven Development for Embedded Software: Application to Communications for Drone Swarm MBIHI Jean Analog Automation and Digital Feedback Control Techniques Advanced Techniques and Technology of Computer-Aided Feedback Control MORANA Joëlle Logistics SIMON Christophe, WEBER Philippe, SALLAK Mohamed Data Uncertainty and Important Measures (Systems Dependability Assessment Set – Volume 3) TANIGUCHI Eiichi, THOMPSON Russell G. City Logistics 1: New Opportunities and Challenges City Logistics 2: Modeling and Planning Initiatives City Logistics 3: Towards Sustainable and Liveable Cities

ZELM Martin, JAEKEL Frank-Walter, DOUMEINGTS Guy, WOLLSCHLAEGER Martin Enterprise Interoperability: Smart Services and Business Impact of Enterprise Interoperability

2017 ANDRÉ Jean-Claude From Additive Manufacturing to 3D/4D Printing 1: From Concepts to Achievements From Additive Manufacturing to 3D/4D Printing 2: Current Techniques, Improvements and their Limitations From Additive Manufacturing to 3D/4D Printing 3: Breakthrough Innovations: Programmable Material, 4D Printing and Bio-printing ARCHIMÈDE Bernard, VALLESPIR Bruno Enterprise Interoperability: INTEROP-PGSO Vision CAMMAN Christelle, FIORE Claude, LIVOLSI Laurent, QUERRO Pascal Supply Chain Management and Business Performance: The VASC Model FEYEL Philippe Robust Control, Optimization with Metaheuristics MARÉ Jean-Charles Aerospace Actuators 2: Signal-by-Wire and Power-by-Wire POPESCU Dumitru, AMIRA Gharbi, STEFANOIU Dan, BORNE Pierre Process Control Design for Industrial Applications RÉVEILLAC Jean-Michel Modeling and Simulation of Logistics Flows 1: Theory and Fundamentals Modeling and Simulation of Logistics Flows 2: Dashboards, Traffic Planning and Management Modeling and Simulation of Logistics Flows 3: Discrete and Continuous Flows in 2D/3D

2016 ANDRÉ Michel, SAMARAS Zissis Energy and Environment (Research for Innovative Transports Set - Volume 1) AUBRY Jean-François, BRINZEI Nicolae, MAZOUNI Mohammed-Habib Systems Dependability Assessment: Benefits of Petri Net Models (Systems Dependability Assessment Set - Volume 1) BLANQUART Corinne, CLAUSEN Uwe, JACOB Bernard Towards Innovative Freight and Logistics (Research for Innovative Transports Set - Volume 2) COHEN Simon, YANNIS George Traffic Management (Research for Innovative Transports Set - Volume 3) MARÉ Jean-Charles Aerospace Actuators 1: Needs, Reliability and Hydraulic Power Solutions REZG Nidhal, HAJEJ Zied, BOSCHIAN-CAMPANER Valerio Production and Maintenance Optimization Problems: Logistic Constraints and Leasing Warranty Services TORRENTI Jean-Michel, LA TORRE Francesca Materials and Infrastructures 1 (Research for Innovative Transports Set Volume 5A) Materials and Infrastructures 2 (Research for Innovative Transports Set Volume 5B) WEBER Philippe, SIMON Christophe Benefits of Bayesian Network Models (Systems Dependability Assessment Set – Volume 2) YANNIS George, COHEN Simon Traffic Safety (Research for Innovative Transports Set - Volume 4)

2015 AUBRY Jean-François, BRINZEI Nicolae Systems Dependability Assessment: Modeling with Graphs and Finite State Automata BOULANGER Jean-Louis CENELEC 50128 and IEC 62279 Standards BRIFFAUT Jean-Pierre E-Enabled Operations Management MISSIKOFF Michele, CANDUCCI Massimo, MAIDEN Neil Enterprise Innovation

2014 CHETTO Maryline Real-time Systems Scheduling Volume 1 – Fundamentals Volume 2 – Focuses DAVIM J. Paulo Machinability of Advanced Materials ESTAMPE Dominique Supply Chain Performance and Evaluation Models FAVRE Bernard Introduction to Sustainable Transports GAUTHIER Michaël, ANDREFF Nicolas, DOMBRE Etienne Intracorporeal Robotics: From Milliscale to Nanoscale MICOUIN Patrice Model Based Systems Engineering: Fundamentals and Methods MILLOT Patrick Designing HumanMachine Cooperation Systems NI Zhenjiang, PACORET Céline, BENOSMAN Ryad, RÉGNIER Stéphane Haptic Feedback Teleoperation of Optical Tweezers

OUSTALOUP Alain Diversity and Non-integer Differentiation for System Dynamics REZG Nidhal, DELLAGI Sofien, KHATAD Abdelhakim Joint Optimization of Maintenance and Production Policies STEFANOIU Dan, BORNE Pierre, POPESCU Dumitru, FILIP Florin Gh., EL KAMEL Abdelkader Optimization in Engineering Sciences: Metaheuristics, Stochastic Methods and Decision Support

2013 ALAZARD Daniel Reverse Engineering in Control Design ARIOUI Hichem, NEHAOUA Lamri Driving Simulation CHADLI Mohammed, COPPIER Hervé Command-control for Real-time Systems DAAFOUZ Jamal, TARBOURIECH Sophie, SIGALOTTI Mario Hybrid Systems with Constraints FEYEL Philippe Loop-shaping Robust Control FLAUS Jean-Marie Risk Analysis: Socio-technical and Industrial Systems FRIBOURG Laurent, SOULAT Romain Control of Switching Systems by Invariance Analysis: Application to Power Electronics GROSSARD Mathieu, REGNIER Stéphane, CHAILLET Nicolas Flexible Robotics: Applications to Multiscale Manipulations GRUNN Emmanuel, PHAM Anh Tuan Modeling of Complex Systems: Application to Aeronautical Dynamics

HABIB Maki K., DAVIM J. Paulo Interdisciplinary Mechatronics: Engineering Science and Research Development HAMMADI Slim, KSOURI Mekki Multimodal Transport Systems JARBOUI Bassem, SIARRY Patrick, TEGHEM Jacques Metaheuristics for Production Scheduling KIRILLOV Oleg N., PELINOVSKY Dmitry E. Nonlinear Physical Systems LE Vu Tuan Hieu, STOICA Cristina, ALAMO Teodoro, CAMACHO Eduardo F., DUMUR Didier Zonotopes: From Guaranteed State-estimation to Control MACHADO Carolina, DAVIM J. Paulo Management and Engineering Innovation MORANA Joëlle Sustainable Supply Chain Management SANDOU Guillaume Metaheuristic Optimization for the Design of Automatic Control Laws STOICAN Florin, OLARU Sorin Set-theoretic Fault Detection in Multisensor Systems

2012 AÏT-KADI Daoud, CHOUINARD Marc, MARCOTTE Suzanne, RIOPEL Diane Sustainable Reverse Logistics Network: Engineering and Management BORNE Pierre, POPESCU Dumitru, FILIP Florin G., STEFANOIU Dan Optimization in Engineering Sciences: Exact Methods CHADLI Mohammed, BORNE Pierre Multiple Models Approach in Automation: Takagi-Sugeno Fuzzy Systems DAVIM J. Paulo Lasers in Manufacturing

DECLERCK Philippe Discrete Event Systems in Dioid Algebra and Conventional Algebra DOUMIATI Moustapha, CHARARA Ali, VICTORINO Alessandro, LECHNER Daniel Vehicle Dynamics Estimation using Kalman Filtering: Experimental Validation GUERRERO José A, LOZANO Rogelio Flight Formation Control HAMMADI Slim, KSOURI Mekki Advanced Mobility and Transport Engineering MAILLARD Pierre Competitive Quality Strategies MATTA Nada, VANDENBOOMGAERDE Yves, ARLAT Jean Supervision and Safety of Complex Systems POLER Raul et al. Intelligent Non-hierarchical Manufacturing Networks TROCCAZ Jocelyne Medical Robotics YALAOUI Alice, CHEHADE Hicham, YALAOUI Farouk, AMODEO Lionel Optimization of Logistics ZELM Martin et al. Enterprise Interoperability –I-EASA12 Proceedings

2011 CANTOT Pascal, LUZEAUX Dominique Simulation and Modeling of Systems of Systems DAVIM J. Paulo Mechatronics DAVIM J. Paulo Wood Machining

GROUS Ammar Applied Metrology for Manufacturing Engineering KOLSKI Christophe Human–Computer Interactions in Transport LUZEAUX Dominique, RUAULT Jean-René, WIPPLER Jean-Luc Complex Systems and Systems of Systems Engineering ZELM Martin, et al. Enterprise Interoperability: IWEI2011 Proceedings

2010 BOTTA-GENOULAZ Valérie, CAMPAGNE Jean-Pierre, LLERENA Daniel, PELLEGRIN Claude Supply Chain Performance / Collaboration, Alignement and Coordination BOURLÈS Henri, GODFREY K.C. Kwan Linear Systems BOURRIÈRES Jean-Paul Proceedings of CEISIE’09 CHAILLET Nicolas, REGNIER Stéphane Microrobotics for Micromanipulation DAVIM J. Paulo Sustainable Manufacturing GIORDANO Max, MATHIEU Luc, VILLENEUVE François Product Life-Cycle Management / Geometric Variations LOZANO Rogelio Unmanned Aerial Vehicles / Embedded Control LUZEAUX Dominique, RUAULT Jean-René Systems of Systems VILLENEUVE François, MATHIEU Luc Geometric Tolerancing of Products

2009 DIAZ Michel Petri Nets / Fundamental Models, Verification and Applications OZEL Tugrul, DAVIM J. Paulo Intelligent Machining PITRAT Jacques Artificial Beings

2008 ARTIGUES Christian, DEMASSEY Sophie, NERON Emmanuel Resources–Constrained Project Scheduling BILLAUT Jean-Charles, MOUKRIM Aziz, SANLAVILLE Eric Flexibility and Robustness in Scheduling DOCHAIN Denis Bioprocess Control LOPEZ Pierre, ROUBELLAT François Production Scheduling THIERRY Caroline, THOMAS André, BEL Gérard Supply Chain Simulation and Management

2007 DE LARMINAT

Philippe Analysis and Control of Linear Systems

DOMBRE Etienne, KHALIL Wisama Robot Manipulators LAMNABHI Françoise et al. Taming Heterogeneity and Complexity of Embedded Control LIMNIOS Nikolaos Fault Trees

2006 FRENCH COLLEGE OF METROLOGY Metrology in Industry NAJIM Kaddour Control of Continuous Linear Systems