Studies in Computational Intelligence 707

Anis Koubaa Editor

Robot

Operating

System (ROS)

The Complete Reference (Volume 2)

Studies in Computational Intelligence

Volume 707

Series editor

Janusz Kacprzyk, Polish Academy of Sciences, Warsaw, Poland

e-mail: kacprzyk@ibspan.waw.pl

About this Series

The series “Studies in Computational Intelligence” (SCI) publishes new developments and advances in the various areas of computational intelligence—quickly and

with a high quality. The intent is to cover the theory, applications, and design

methods of computational intelligence, as embedded in the ﬁelds of engineering,

computer science, physics and life sciences, as well as the methodologies behind

them. The series contains monographs, lecture notes and edited volumes in

computational intelligence spanning the areas of neural networks, connectionist

systems, genetic algorithms, evolutionary computation, artiﬁcial intelligence,

cellular automata, self-organizing systems, soft computing, fuzzy systems, and

hybrid intelligent systems. Of particular value to both the contributors and the

readership are the short publication timeframe and the worldwide distribution,

which enable both wide and rapid dissemination of research output.

More information about this series at http://www.springer.com/series/7092

Anis Koubaa

Editor

Robot Operating System

(ROS)

The Complete Reference (Volume 2)

Special focus on Unmanned Aerial Vehicles (UAVs) with

ROS

123

Editor

Anis Koubaa

Prince Sultan University

Riyadh

Saudi Arabia

and

CISTER Research Unit

Porto

Portugal

and

Gaitech Robotics

Hong Kong

China

ISSN 1860-949X

ISSN 1860-9503 (electronic)

Studies in Computational Intelligence

ISBN 978-3-319-54926-2

ISBN 978-3-319-54927-9 (eBook)

DOI 10.1007/978-3-319-54927-9

Library of Congress Control Number: 2017933861

© Springer International Publishing AG 2017

This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part

of the material is concerned, speciﬁcally the rights of translation, reprinting, reuse of illustrations,

recitation, broadcasting, reproduction on microﬁlms or in any other physical way, and transmission

or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar

methodology now known or hereafter developed.

The use of general descriptive names, registered names, trademarks, service marks, etc. in this

publication does not imply, even in the absence of a speciﬁc statement, that such names are exempt from

the relevant protective laws and regulations and therefore free for general use.

The publisher, the authors and the editors are safe to assume that the advice and information in this

book are believed to be true and accurate at the date of publication. Neither the publisher nor the

authors or the editors give a warranty, express or implied, with respect to the material contained herein or

for any errors or omissions that may have been made. The publisher remains neutral with regard to

jurisdictional claims in published maps and institutional afﬁliations.

Printed on acid-free paper

This Springer imprint is published by Springer Nature

The registered company is Springer International Publishing AG

The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

Acknowledgements

The Editor would like to thank the Robotics and Internet of Things (RIoT) Unit at

Center of Excellence of Prince Sultan University for their support to this work.

Furthermore, the Editor thanks Gaitech Robotics in China for their support.

v

Acknowledgements to Reviewers

The Editor would like to thank the following reviewers for their great contributions

in the review process of the book by providing a quality feedback to authors.

Anis

Koubâa

Francisco

Grau

Michael

Bence

Maram

Marc

Andre

Marco

Walter

Péter

Christoph

Carroll

Magyar

Alajlan

Morenza-Cinos

Oliveira

Wehrmeister

Fetter Lages

Fankhauser

Rösmann

Francesco

Christopher-Eyk

Guilherme

Andreas

Juan

Timo

Zavier

Myrel

Junhao

Huimin

Alfredo

Dinesh

Rovida

Hrabia

Sousa Bastos

Bihlmaier

Jimeno

Röhling

Lee

Alsayegh

Xiao

Lu

Soto

Madusanke

Prince Sultan University, Saudi Arabia/CISTER

Research Unit, Portugal

CATEC (Center for Advanced Aerospace

Technologies)

Robotic Paradigm Systems

PAL Robotics

Al-Imam Mohamed bin Saud University

UPF

UTFPR

Federal University of Technology – Parana

Universidade Federal do Rio Grande do Sul

ETH Zurich

Institute of Control Theory and Systems Engineering,

TU Dortmund University

Aalborg University of Copenhagen

Technische Universität/DAI Labor

UNIFEI

Karlsruhe Institute of Technologie (KIT)

linorobot.org

Fraunhofer FKIE

Henan University of Science and Technology

RST-TU Dortmund

National University of Defense Technology

National University of Defense Technology

Freescale Semiconductors

University of Moratuwa

(continued)

vii

viii

Acknowledgements to Reviewers

(continued)

Roberto

Ingo

Brad

Yasir

Mohamed-Foued

Murilo

Guzman

Lütkebohle

Bazemore

Javed

Sriti

Martins

Robotnik

Robert Bosch GmbH

University of Georgia

Prince Sultan University, Saudi Arabia

Al-Imam Muhammad Ibn Saud Islamic University

Centro Universitario da FEI

Contents

Part I

Control of UAVs

Model Predictive Control for Trajectory Tracking

of Unmanned Aerial Vehicles Using Robot Operating System. . . . . . . . .

Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

3

Designing Fuzzy Logic Controllers for ROS-Based Multirotors . . . . . . .

Emanoel Koslosky, André Schneider de Oliveira,

Marco Aurélio Wehrmeister and João Alberto Fabro

41

Flying Multiple UAVs Using ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Wolfgang Hönig and Nora Ayanian

83

Part II

Control of Mobile Robots

SkiROS—A Skill-Based Robot Control Platform on Top

of ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121

Francesco Rovida, Matthew Crosby, Dirk Holz,

Athanasios S. Polydoros, Bjarne Großmann,

Ronald P.A. Petrick and Volker Krüger

Control of Mobile Robots Using ActionLib . . . . . . . . . . . . . . . . . . . . . . . . 161

Higor Barbosa Santos, Marco Antônio Simões Teixeira,

André Schneider de Oliveira, Lúcia Valéria Ramos de Arruda

and Flávio Neves, Jr.

Parametric Identiﬁcation of the Dynamics of Mobile Robots

and Its Application to the Tuning of Controllers in ROS . . . . . . . . . . . . 191

Walter Fetter Lages

Online Trajectory Planning in ROS Under Kinodynamic

Constraints with Timed-Elastic-Bands . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231

Christoph Rösmann, Frank Hoffmann and Torsten Bertram

ix

x

Part III

Contents

Integration of ROS with Internet and Distributed Systems

ROSLink: Bridging ROS with the Internet-of-Things for Cloud

Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265

Anis Koubaa, Maram Alajlan and Basit Qureshi

ROS and Docker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285

Rufﬁn White and Henrik Christensen

A ROS Package for Dynamic Bandwidth Management

in Multi-robot Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 309

Ricardo Emerson Julio and Guilherme Sousa Bastos

Part IV

Service Robots and Fields Experimental

An Autonomous Companion UAV for the SpaceBot

Cup Competition 2015 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345

Christopher-Eyk Hrabia, Martin Berger, Axel Hessler,

Stephan Wypler, Jan Brehmer, Simon Matern and Sahin Albayrak

Development of an RFID Inventory Robot (AdvanRobot) . . . . . . . . . . . . 387

Marc Morenza-Cinos, Victor Casamayor-Pujol, Jordi Soler-Busquets,

José Luis Sanz, Roberto Guzmán and Rafael Pous

Robotnik—Professional Service Robotics Applications

with ROS (2) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 419

Roberto Guzmán, Román Navarro, Miquel Cantero

and Jorge Ariño

Using ROS in Multi-robot Systems: Experiences

and Lessons Learned from Real-World Field Tests . . . . . . . . . . . . . . . . . 449

Mario Garzón, João Valente, Juan Jesús Roldán,

David Garzón-Ramos, Jorge de León, Antonio Barrientos

and Jaime del Cerro

Part V

Perception and Sensing

Autonomous Navigation in a Warehouse

with a Cognitive Micro Aerial Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . 487

Marius Beul, Nicola Krombach, Matthias Nieuwenhuisen,

David Droeschel and Sven Behnke

Robots Perception Through 3D Point Cloud Sensors . . . . . . . . . . . . . . . . 525

Marco Antonio Simões Teixeira, Higor Barbosa Santos,

André Schneider de Oliveira, Lucia Valeria Arruda and Flavio Neves, Jr.

Contents

Part VI

xi

ROS Simulation Frameworks

Environment for the Dynamic Simulation of ROS-Based UAVs . . . . . . . 565

Alvaro Rogério Cantieri, André Schneider de Oliveira,

Marco Aurélio Wehrmeister, João Alberto Fabro

and Marlon de Oliveira Vaz

Building Software System and Simulation Environment for RoboCup

MSL Soccer Robots Based on ROS and Gazebo . . . . . . . . . . . . . . . . . . . 597

Junhao Xiao, Dan Xiong, Weijia Yao, Qinghua Yu, Huimin Lu

and Zhiqiang Zheng

VIKI—More Than a GUI for ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 633

Robin Hoogervorst, Cees Trouwborst, Alex Kamphuis

and Matteo Fumagalli

Editor and Contributors

About the Editor

Anis Koubaa is a full professor in Computer Science at Prince Sultan University and research

associate in CISTER Research Unit, ISEP-IPP, Portugal, add Senior Research Consultant with

Gaitech Robotics, China. He becomes a Senior Fellow of the Higher Education Academy

(SFHEA) in 2015. He received his B.Sc. in Telecommunications Engineering from Higher School

of Telecommunications (Tunisia), and M.Sc. degrees in Computer Science from University Henri

Poincare (France), in 2000 and 2001, respectively, and the Ph.D. degree in Computer Science

from the National Polytechnic Institute of Lorraine (France), in 2004. He was a faculty member at

Al-Imam University from 2006 to 2012. He has published over 120 refereed journal and conference papers. His research interest covers mobile robots, cloud robotics, robotics software

engineering, Internet-of-Things, cloud computing and wireless sensor networks. Dr. Anis received

the best research award from Al-Imam University in 2010, and the best paper award of the 19th

Euromicro Conference in Real-Time Systems (ECRTS) in 2007. He is the head of the ACM

Chapter in Prince Sultan University. His H-Index is 30.

Contributors

Maram Alajlan Center of Excellence Robotics and Internet of Things (RIOT)

Research Unit, Prince Sultan University, Riyadh, Saudi Arabia; King Saud

University, Riyadh, Saudi Arabia

Sahin Albayrak DAI-Labor, Technische Universität Berlin, Berlin, Germany

Kostas Alexis University of Nevada, Reno, NV, USA

Jorge Ariño Robotnik Automation, SLL, Ciutat de Barcelona, Paterna, Valencia,

Spain

Lucia Valeria Arruda Federal University of Technology—Parana, Curitiba,

Brazil

Nora Ayanian Department of Computer Science, University of Southern

California, Los Angeles, CA, USA

xiii

xiv

Editor and Contributors

Antonio Barrientos Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Guilherme Sousa Bastos System Engineering and Information Technology

Institute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

MG, Brazil

Sven Behnke Autonomous Intelligent Systems Group, University of Bonn, Bonn,

Germany

Martin Berger DAI-Labor, Technische Universität Berlin, Berlin, Germany

Torsten Bertram Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Marius Beul Autonomous Intelligent Systems Group, University of Bonn, Bonn,

Germany

Jan Brehmer DAI-Labor, Technische Universität Berlin, Berlin, Germany

Miquel Cantero Robotnik Automation, SLL, Ciutat de Barcelona, Paterna,

Valencia, Spain

Alvaro Rogério Cantieri Federal Institute of Parana, Curitiba, Brazil

Victor Casamayor-Pujol Universtitat Pompeu Fabra, Barcelona, Spain

Henrik Christensen Contextual Robotics Institute, University of California, San

Diego, CA, USA

Matthew Crosby Heriot-Watt University, Edinburgh, UK

Lúcia Valéria Ramos de Arruda Federal University of Technology—Parana,

Curitiba, Brazil

Jorge de León Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

André Schneider de Oliveira Advanced Laboratory of Embedded Systems and

Robotics (LASER), Federal University of Technology—Parana (UTFPR), Curitiba,

Brazil

Marlon de Oliveira Vaz Federal Institute of Parana, Curitiba, Brazil

Jaime del Cerro Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

David Droeschel Autonomous Intelligent Systems Group, University of Bonn,

Bonn, Germany

João Alberto Fabro Advanced Laboratory of Embedded Systems and Robotics

(LASER), Federal University of Technology—Parana (UTFPR), Curitiba, Brazil

Matteo Fumagalli Aalborg University, Copenhagen, Denmark

David Garzón-Ramos Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Editor and Contributors

xv

Mario Garzón Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

Bjarne Großmann Aalborg University Copenhagen, Copenhagen, Denmark

Roberto Guzmán Robotnik Automation S.L.L., Paterna, Valencia, Spain

Axel Hessler DAI-Labor, Technische Universität Berlin, Berlin, Germany

Frank Hoffmann Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Dirk Holz Bonn University, Bonn, Germany

Robin Hoogervorst University of Twente, Enschede, Netherlands

Christopher-Eyk Hrabia DAI-Labor, Technische Universität Berlin, Berlin,

Germany

Wolfgang Hönig Department of Computer Science, University of Southern

California, Los Angeles, CA, USA

Ricardo Emerson Julio System Engineering and Information Technology

Institute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

MG, Brazil

Mina Kamel Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Alex Kamphuis University of Twente, Enschede, Netherlands

Emanoel Koslosky Advanced Laboratory of Embedded Systems and Robotics

(LASER), Federal University of Technology—Parana (UTFPR), Curitiba, Brazil

Anis Koubaa Center of Excellence Robotics and Internet of Things (RIOT)

Research Unit, Prince Sultan University, Riyadh, Saudi Arabia; Gaitech Robotics,

Hong Kong, China; CISTER/INESC-TEC, ISEP, Polytechnic Institute of Porto,

Porto, Portugal

Nicola Krombach Autonomous Intelligent Systems Group, University of Bonn,

Bonn, Germany

Volker Krüger Aalborg University Copenhagen, Copenhagen, Denmark

Walter Fetter Lages Federal University of Rio Grande do Sul, Porto Alegre RS,

Brazil

Huimin Lu College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Simon Matern Technische Universität Berlin, Berlin, Germany

Marc Morenza-Cinos Universtitat Pompeu Fabra, Barcelona, Spain

Román Navarro Robotnik Automation, SLL, Ciutat de Barcelona, Paterna,

Valencia, Spain

xvi

Editor and Contributors

Flávio Neves Jr. Federal University of Technology—Parana, Curitiba, Brazil

Matthias Nieuwenhuisen Autonomous Intelligent Systems Group, University of

Bonn, Bonn, Germany

Ronald P.A. Petrick Heriot-Watt University, Edinburgh, UK

Athanasios

Denmark

S.

Polydoros Aalborg

University

Copenhagen,

Copenhagen,

Rafael Pous Universtitat Pompeu Fabra, Barcelona, Spain

Basit Qureshi Prince Sultan University, Riyadh, Saudi Arabia

Juan Jesús Roldán Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Francesco Rovida Aalborg University Copenhagen, Copenhagen, Denmark

Christoph Rösmann Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Higor Barbosa Santos Federal University of Technology—Parana, Curitiba,

Brazil

José Luis Sanz Keonn Technologies S.L., Barcelona, Spain

Roland Siegwart Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Jordi Soler-Busquets Universtitat Pompeu Fabra, Barcelona, Spain

Thomas Stastny Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Marco Antonio Simões Teixeira Federal University of Technology—Parana,

Curitiba, Brazil

Cees Trouwborst University of Twente, Enschede, Netherlands

João Valente Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

Marco Aurélio Wehrmeister Advanced Laboratory of Embedded Systems and

Robotics (LASER), Federal University of Technology—Parana (UTFPR), Curitiba,

Brazil

Rufﬁn White Contextual Robotics Institute, University of California, San Diego,

CA, USA

Stephan Wypler Technische Universität Berlin, Berlin, Germany

Junhao Xiao College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Dan Xiong College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Editor and Contributors

xvii

Weijia Yao College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Qinghua Yu College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Zhiqiang Zheng College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Part I

Control of UAVs

Model Predictive Control for Trajectory

Tracking of Unmanned Aerial Vehicles

Using Robot Operating System

Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

Abstract In this chapter, strategies for Model Predictive Control (MPC) design and

implementation for Unmaned Aerial Vehicles (UAVs) are discussed. This chapter is

divided into two main sections. In the first section, modelling, controller design and

implementation of MPC for multi-rotor systems is presented. In the second section,

we show modelling and controller design techniques for fixed-wing UAVs. System

identification techniques are used to derive an estimate of the system model, while

state of the art solvers are employed to solve the optimization problem online. By

the end of this chapter, the reader should be able to implement an MPC to achieve

trajectory tracking for both multi-rotor systems and fixed-wing UAVs.

1 Introduction

Aerial robots are gaining great attention recently as they have many advantages

over ground robots to execute inspection, search and rescue, surveillance and goods

delivery tasks. Depending on the task required to be executed, a multi-rotor system

or fixed-wing aircraft might be a more suitable choice. For instance, a fixed-wing

aircraft is more suitable for surveillance and large-scale mapping tasks thanks to

their long endurance capability and higher speed compared to a multi-rotor system,

while for an inspection task that requires flying close to structures to obtain detailed

footage a multi-rotor UAV is more appropriate.

M. Kamel (B) · T. Stastny · R. Siegwart

Autonomous System Lab, ETH Zurich, Zurich, Switzerland

e-mail: fmina@ethz.ch

T. Stastny

e-mail: tstastny@ethz.ch

R. Siegwart

e-mail: rsiegwart@ethz.ch

K. Alexis

University of Nevada, Reno, NV, USA

e-mail: kalexis@unr.edu

© Springer International Publishing AG 2017

A. Koubaa (ed.), Robot Operating System (ROS), Studies in Computational

Intelligence 707, DOI 10.1007/978-3-319-54927-9_1

3

4

M. Kamel et al.

Precise trajectory tracking is a demanding feature for aerial robots in general in

order to successfully perform required tasks, especially when operating in realistic

environments where external disturbances may heavily affect the flight performance

and when flying in the vicinity of structure. In this chapter, several model predictive

control strategies for trajectory tracking are presented for multi-rotor systems as well

as for fixed-wing aircraft. The general control structure followed by this chapter is a

cascade control approach, where a reliable and system-specific low-level controller

is present as inner loop, and a model-based trajectory tracking controller is running

as an outer loop. This approach is motivated by the fact that many critical flight

software is running on a separate navigation hardware which is typically based on

micro-controllers, such as Pixhawk PX4 and Navio [1, 2] while high level tasks are

running on more powerful -but less reliable- on-board computers. This introduces a

separation layer to keep critical tasks running despite any failure in the more complex

high-level computer.

By the end of this chapter, the reader should be able to implement and test various

Model Predictive Control strategies for aerial robots trajectory tracking, and integrate

these controllers into the Robot Operating System (ROS) [3]. Various implementation

hints and practical suggestions are provided in this chapter and we show several

experimental results to evaluate the proposed control algorithms on real systems.

In Sect. 2 the general theory behind MPC is presented, with focus on linear MPC

and robust linear MPC. In Sect. 3 we present the multi-rotor system model and

give hints on model identification approaches. Moreover, linear and robust MPC

are presented and we show how to integrate these controller into ROS and present

experimental validation results. In Sect. 4 we present a Nonlinear MPC approach for

lateral-directional position control of fixed-wing aircraft with implementation hints

and validation experiments.

2 Background

2.1 Concepts of Receding Horizon Control

Receding Horizon Control (RHC) corresponds to the historic evolution in control

theory that aimed to attack the known challenges of fixed horizon control. Fixed

horizon optimization computes a sequence of control actions {u 0 , u 1 , . . . , u N −1 }

over a horizon N and is characterized by two main drawbacks, namely: (a) when

an unexpected (unknown during the control design phase) disturbance takes place

or when the model employed for control synthesis behaves different than the actual

system, then the controller has no way to account for that over the computed control

sequence, and (b) as one approaches the final control steps (over the computer fixed

horizon) the control law “gives up trying” since there is too little time left in the fixed

horizon to go to achieve a significant objective function reduction. To address these

limitations, RHC proposed the alternative strategy of computing the full control

Model Predictive Control for Trajectory Tracking …

5

sequence, applying only the first step of it and then repeating the whole process

iteratively (receding horizon fashion). RHC strategies are in general applicable to

nonlinear dynamics of the form (considering that state update is available):

x˙ = f(x, u)

(1)

where the vector field f : Rn × Rm , x ∈ Rn×1 represents the state vector, and u ∈

Rm×1 the input vector. The general state feedback-based RHC optimization problem

takes the following form:

N −1

ref

min F(xt+N ) +

z

s.t.

||xt+k − xt+k || + ||ut+k ||

k=0

xt+k+1 = f (xt+k , ut+k )

ut+k ∈ UC

xt+k ∈ XC

xt = x(t)

(2)

where z = {ut , ut+1 , . . . , ut+N −1 } is the optimization variables, denotes some

(penalized) metric used for per-stage weighting, F(xt+N ) represents the terminal

ref

state weighting, xt+k is the reference signal, the subscript t + k is used to denote the

sample (using a fixed sampling time Ts ) of a signal at k steps ahead of the current

time t, while t + k + 1 indicates the next evolution of that, UC represents the set of

input constraints, XC the state constraints and x(t) is the value of the state vector at

the beginning of the current RHC iteration. The solution of this optimization problem

leads again to an optimal control sequence {ut , ut+1 , . . . , ut+N −1 } but only the first

step of that ut is applied while the whole process is then repeated iteratively.

Within this formulation, the term F(xt+N ) has a critical role for the closed-loop

stability. In particular, it forces the system state to take values within a particular set

at the end of the prediction horizon. It is relatively easy to prove stability per local

iteration using Lyapunov analysis. In its simplest case, this essentially means that

ref

considering the regulation problem (xt+k = 0 for k = 0, . . . , N − 1), and a “decrescent” metric , then the solution of the above optimization problem makes the system

stable at xt = 0, ut = 0 – that is that a terminal constraint xt+k = 0 is introduced (a

simplified illustration is provided in Fig. 1). However, the question of global stability

Fig. 1 Illustration of the terminal constraint set (Ω)

6

M. Kamel et al.

is in general not guaranteed. For that one has to consider the problem of introducing

both a terminal cost and a terminal constraint for the states [4]. However, general

constrained optimization problems can be extremely difficult to solve, and simply

adding terminal constraints may not be feasible. Note that in many practical cases,

the terminal constraint is not enforced during the control design procedure, but rather

verified a posteriori (by increasing the prediction horizon if not satisfied).

Furthermore, one of the most challenging properties of RHC is that of recursive feasibility. Unfortunately, although absolutely recommended from a theoretical

standpoint, it is not always possible to construct a RHC that has a-priori guarantee

of recursive feasibility, either due to theoretical or practical implications. In general,

a RHC strategy lacks recursive feasibility –and is therefore invalidated– even when

it is possible to find a state which is feasible, but where the optimal control action

moves the state vector to a point where the RHC optimization problem is infeasible.

Although a general feasibility analysis methodology is very challenging, for specific cases powerful tools exist. In particular, for the case of linear systems then the

Farkas’ Lemma [5] in combination with bilevel programming can be used to search

for problematic initial states which lack recursive feasibility – thus invalidating an

RHC strategy.

2.2 Linear Model Predictive Control

In this subsection we briefly present the theory behind MPC for linear systems. We

formulate the optimal control problem for linear systems with linear constraints in the

input and state variables. Moreover, we discuss the control input properties, stability

and feasibility in the case of linear and quadratic cost function. To achieve offset

free tracking under model mismatch, we adopt the approach described in [6] where

the system model is augmented with additional disturbances state d(t) to capture

the model mismatch. An observer is employed to estimate disturbances in steady

state. The observer design and the disturbance model will be briefly discussed in this

subsection.

J0 x0 , U, Xr e f , Ur e f

min

U

subject to xk+1 = Axk + Buk + Bd dk ;

dk+1 = dk , k = 0, . . . , N − 1

xk ∈ XC , uk ∈ UC

x N ∈ XC N

x0 = x (t0 ) , d0 = d (t0 ) .

(3)

The optimal control problem to achieve offset-free state tracking under linear

state and and input constraints is shown in (3), where J0 is the cost function, Xr e f =

ref

ref

{x0 , . . . , x N } is the reference state sequence, U = {u0 , . . . , u N −1 } and Ur e f =

ref

ref

{u0 , . . . , u N −1 } are respectively the control input sequence and the steady state

input sequence, Bd is the disturbance model and dk is the external disturbances, XC ,

Model Predictive Control for Trajectory Tracking …

7

UC and XC N are polyhedra. The choice of the disturbance model is not a trivial

task, and depends on the system under consideration and the type of disturbances

expected. The optimization problem is defined as

N −1

J0 x0 , U, X

ref

,U

ref

ref

ref

ref

ref

(xk − xk )T Qx (xk − xk )+

=

k=0

(uk − uk )T Ru (uk − uk )+

(4)

(uk − uk−1 ) RΔ (uk − uk−1 ) +

T

ref

ref

(x N − x N )T P(x N − x N ),

where Qx 0 is the penalty on the state error, Ru 0 is the penalty on control input

error, RΔ 0 is a penalty on the control change rate and P is the terminal state error

penalty.

In general, stability and feasibility of receding horizon problems are not ensured

except for particular cases such as infinite horizon control problems as in Linear

Quadratic Regulator (LQR) case. When the prediction horizon is limited to N steps,

the stability and feasibility guarantees are disputable. In principle, longer prediction

horizon tends to improve stability and feasibility properties of the controller, but the

computation effort will increase, and for aerial robot application, fast control action

needs to be computed on limited computation power platforms. However, the terminal

cost P and terminal constraint XC N can be chosen such that closed-loop stability and

feasibility are ensured [6]. In this chapter we focus more on the choice of terminal

weight P as it is easy to compute, while the terminal constraint is generally more

difficult and practically stability is achieved with long enough prediction horizon.

Note that in our cost function (4), we penalize the control input rate Δuk . This

ensures smooth control input and avoids undesired oscillations. In the cost function (4), u−1 is the actual control input applied on the system in the previous time

step.

As previously mentioned, offset-free reference tracking can be achieved by

augmenting the system model with disturbances dk to capture the modeling error.

Assuming that we want to track the system output yk = Cxk and achieve steady state

offset free tracking y∞ = r∞ . A simple observer that can estimate such disturbance

can be achieved as follows

xˆ k+1

dˆ k+1

=

A Bd

0 I

xˆ k

dˆ k

+

B

Lx

uk +

Ld

0

Cˆxk − ym,k

(5)

where xˆ and dˆ are the estimated state and external disturbances, Lx and Ld are the

observer gains and ym,k is the measured output at time k.

Under the assumption of stable observer, it is possible to compute the MPC state

at steady state xr e f and control input at steady state ur e f by solving the following

system of linear equations:

8

M. Kamel et al.

A−I B

C 0

xr e f,k

ur e f,k

=

−Bd dˆ k

rk

(6)

2.3 Nonlinear Model Predictive Control

Aerial vehicles behavior is better described by a set of nonlinear differential equations

to capture the aerodynamic and coupling effects. Therefore in this subsection we

present the theory behind Nonlinear MPC that exploits the full system dynamics,

and generally achieve better performance when it comes to aggressive trajectory

tracking. The optimization problem for nonlinear MPC is formulated in Eq. (7).

T

min

U

h (x(t), u(t)) − yr e f (t)

t=0

subject to

2

Q

dt + m (x(T )) − yr e f (T )

x˙ = f(x(t), u(t));

u(t) ∈ UC

x(t) ∈ XC

2

P

(7)

x(0) = x (t0 ) .

A direct multiple shooting technique is used to solve the Optimal Control Problem

(OCP) (7). In this approach the system dynamics are discretized over a time grid

t0 , . . . , t N within the time intervals tk , tk+1 . The inequality constraints and control

action are discretized over the same time grid. A Boundary Value Problem (BVP)

is solved for each interval and additional continuity constraints are imposed. Due

to the nature of the system dynamics and the imposed constraints, the optimization

problem becomes a Nonlinear Program (NLP). This NLP is solved using Sequential

Quadratic Programming (SQP) technique where the Quadratic Programs (QPs) are

solved by active set method using the qpOASES solver [7].

Note that, in case of infeasibility of the underlying QP, 1 penalized slack variables

are introduced to relax all constraints.

The controller is implemented in a receding horizon fashion where only the first

computed control action is applied to the system, and the rest of the predicted state

and control trajectory is used as initial guess for the OCP to solve in the next iteration.

2.4 Linear Robust Model Predictive Control

Despite the robustness properties of the nominal MPC formulation, specific robust

control variations exist when further robustness guarantees are required. The problem

of linear Robust Model Predictive Control (RMPC) may be formulated as a Minimax optimization problem that is solved explicitly. As an optimality metric we may

Model Predictive Control for Trajectory Tracking …

9

Relaxations - Derivation of Convex Optimization Problem

Feedback

Predictions

Objective Function

State Space

representation

using the concatenated vectors over

the prediction

horizon

Constraints Robustification

State and Input Constraints

Multiparametric

Optimizer

Explicit

Piecewise Affine

form

Extended

Sequential Table

Traversal

Fig. 2 Overview of the explicit RMPC optimization problem functional components

select the Minimum Peak Performance Measure (MPPM) for its known robustness

properties. Figure 2 outlines the relevant building blocks [8].

Within this RMPC approach, the following linear time invariant representation of

the system dynamics may be considered:

xk+1 = Axk + Buk + Gwk

yk+1 = Cxk

(8)

where xk ∈ X, uk ∈ U and the disturbing signals wk are unknown but bounded (wk ∈

W). Within this paper, box-constrained disturbances are considered (W∞ = {w :

||w||∞ ≤ 1}). Consequently, the RMPC problem will be formulated for the system

representation and additive disturbance presented above. Let the following denote

the concatenated versions of the predicted output, states, inputs and disturbances,

where [k + i|k] marks the values profile at time k + i, from time k.

T yT

T

Y = yk|k

k+1|k . . . yk+N −1|k

(9)

T xT

T

X = xk|k

k+1|k . . . xk+N −1|k

(10)

U=

T

uk|k

T

uk+1|k

W=

T

wk|k

T

wk+1|k

...

...

T

uk+N

−1|k

T

wk+N

−1|k

(11)

(12)

10

M. Kamel et al.

where X ∈ X N = X × X · · · × X, U ∈ U N = U × U · · · × U, W ∈ W N = W ×

W × · · · × W. The predicted states and outputs present linear dependency on the

current state, the future control input and the disturbance, and thus the following

holds:

X = Axk|k + BU + GW

Y = CX

(13)

where A, B , C , G are the stacked state vector matrices as in [8]. Subsequently, the

RMPC problem based on the MPPM (MPPM–RMPC) may be formulated as:

min max ||Y ||∞ , ||Y ||∞ = max ||yk+ j|k ||∞

u

w

j

(14)

s.t. uk+ j|k ∈ U, ∀ w ∈ W

xk+ j|k ∈ X, ∀ w ∈ W

wk+ j|k ∈ W

2.4.1

Feedback Predictions

Following the aforementioned formulation, the optimization problem will tend to

become conservative as the optimization essentially computes an open-loop control

sequence. Feedback predictions is a method to encode the knowledge that a receding

horizon approach is followed. Towards their incorporation, a type of feedback control

structure has to be assumed. Among the feedback parametrizations that are known

to lead to a convex problem space, the following is selected [9, 10]:

T υT

T

U = LW + V , V = υk|k

k+1|k · · · υk+N −1|k

⎛

⎞

0

0

0

···

0

⎜ L 10

0

0

···

0⎟

⎜

⎟

⎜ L 20

⎟

L

0

·

·

·

0

21

TL = ⎜

⎟

⎜

⎟

.

..

..

..

.

.

.

⎝

.

.⎠

.

.

.

L (N −1)0 L (N −1)1 · · · L (N −1)(N −2) 0

(15)

(16)

Employing this feedback predictions parameterization, the control sequence is now

parameterized directly in the uncertainty, and the matrix L describes how the control

action uses the disturbance vector. Inserting this parametrization yields the following

representation, where V becomes now the RMPC-manipulated action:

X = Axk|k + BV + (G + BL)W

U = LW + V

(17)

(18)

and the mapping from L, V to X , U is now bilinear. This allows the formulation of

the minimax MPC as a convex optimization problem [9]. Furthermore, let:

Anis Koubaa Editor

Robot

Operating

System (ROS)

The Complete Reference (Volume 2)

Studies in Computational Intelligence

Volume 707

Series editor

Janusz Kacprzyk, Polish Academy of Sciences, Warsaw, Poland

e-mail: kacprzyk@ibspan.waw.pl

About this Series

The series “Studies in Computational Intelligence” (SCI) publishes new developments and advances in the various areas of computational intelligence—quickly and

with a high quality. The intent is to cover the theory, applications, and design

methods of computational intelligence, as embedded in the ﬁelds of engineering,

computer science, physics and life sciences, as well as the methodologies behind

them. The series contains monographs, lecture notes and edited volumes in

computational intelligence spanning the areas of neural networks, connectionist

systems, genetic algorithms, evolutionary computation, artiﬁcial intelligence,

cellular automata, self-organizing systems, soft computing, fuzzy systems, and

hybrid intelligent systems. Of particular value to both the contributors and the

readership are the short publication timeframe and the worldwide distribution,

which enable both wide and rapid dissemination of research output.

More information about this series at http://www.springer.com/series/7092

Anis Koubaa

Editor

Robot Operating System

(ROS)

The Complete Reference (Volume 2)

Special focus on Unmanned Aerial Vehicles (UAVs) with

ROS

123

Editor

Anis Koubaa

Prince Sultan University

Riyadh

Saudi Arabia

and

CISTER Research Unit

Porto

Portugal

and

Gaitech Robotics

Hong Kong

China

ISSN 1860-949X

ISSN 1860-9503 (electronic)

Studies in Computational Intelligence

ISBN 978-3-319-54926-2

ISBN 978-3-319-54927-9 (eBook)

DOI 10.1007/978-3-319-54927-9

Library of Congress Control Number: 2017933861

© Springer International Publishing AG 2017

This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part

of the material is concerned, speciﬁcally the rights of translation, reprinting, reuse of illustrations,

recitation, broadcasting, reproduction on microﬁlms or in any other physical way, and transmission

or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar

methodology now known or hereafter developed.

The use of general descriptive names, registered names, trademarks, service marks, etc. in this

publication does not imply, even in the absence of a speciﬁc statement, that such names are exempt from

the relevant protective laws and regulations and therefore free for general use.

The publisher, the authors and the editors are safe to assume that the advice and information in this

book are believed to be true and accurate at the date of publication. Neither the publisher nor the

authors or the editors give a warranty, express or implied, with respect to the material contained herein or

for any errors or omissions that may have been made. The publisher remains neutral with regard to

jurisdictional claims in published maps and institutional afﬁliations.

Printed on acid-free paper

This Springer imprint is published by Springer Nature

The registered company is Springer International Publishing AG

The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

Acknowledgements

The Editor would like to thank the Robotics and Internet of Things (RIoT) Unit at

Center of Excellence of Prince Sultan University for their support to this work.

Furthermore, the Editor thanks Gaitech Robotics in China for their support.

v

Acknowledgements to Reviewers

The Editor would like to thank the following reviewers for their great contributions

in the review process of the book by providing a quality feedback to authors.

Anis

Koubâa

Francisco

Grau

Michael

Bence

Maram

Marc

Andre

Marco

Walter

Péter

Christoph

Carroll

Magyar

Alajlan

Morenza-Cinos

Oliveira

Wehrmeister

Fetter Lages

Fankhauser

Rösmann

Francesco

Christopher-Eyk

Guilherme

Andreas

Juan

Timo

Zavier

Myrel

Junhao

Huimin

Alfredo

Dinesh

Rovida

Hrabia

Sousa Bastos

Bihlmaier

Jimeno

Röhling

Lee

Alsayegh

Xiao

Lu

Soto

Madusanke

Prince Sultan University, Saudi Arabia/CISTER

Research Unit, Portugal

CATEC (Center for Advanced Aerospace

Technologies)

Robotic Paradigm Systems

PAL Robotics

Al-Imam Mohamed bin Saud University

UPF

UTFPR

Federal University of Technology – Parana

Universidade Federal do Rio Grande do Sul

ETH Zurich

Institute of Control Theory and Systems Engineering,

TU Dortmund University

Aalborg University of Copenhagen

Technische Universität/DAI Labor

UNIFEI

Karlsruhe Institute of Technologie (KIT)

linorobot.org

Fraunhofer FKIE

Henan University of Science and Technology

RST-TU Dortmund

National University of Defense Technology

National University of Defense Technology

Freescale Semiconductors

University of Moratuwa

(continued)

vii

viii

Acknowledgements to Reviewers

(continued)

Roberto

Ingo

Brad

Yasir

Mohamed-Foued

Murilo

Guzman

Lütkebohle

Bazemore

Javed

Sriti

Martins

Robotnik

Robert Bosch GmbH

University of Georgia

Prince Sultan University, Saudi Arabia

Al-Imam Muhammad Ibn Saud Islamic University

Centro Universitario da FEI

Contents

Part I

Control of UAVs

Model Predictive Control for Trajectory Tracking

of Unmanned Aerial Vehicles Using Robot Operating System. . . . . . . . .

Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

3

Designing Fuzzy Logic Controllers for ROS-Based Multirotors . . . . . . .

Emanoel Koslosky, André Schneider de Oliveira,

Marco Aurélio Wehrmeister and João Alberto Fabro

41

Flying Multiple UAVs Using ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Wolfgang Hönig and Nora Ayanian

83

Part II

Control of Mobile Robots

SkiROS—A Skill-Based Robot Control Platform on Top

of ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121

Francesco Rovida, Matthew Crosby, Dirk Holz,

Athanasios S. Polydoros, Bjarne Großmann,

Ronald P.A. Petrick and Volker Krüger

Control of Mobile Robots Using ActionLib . . . . . . . . . . . . . . . . . . . . . . . . 161

Higor Barbosa Santos, Marco Antônio Simões Teixeira,

André Schneider de Oliveira, Lúcia Valéria Ramos de Arruda

and Flávio Neves, Jr.

Parametric Identiﬁcation of the Dynamics of Mobile Robots

and Its Application to the Tuning of Controllers in ROS . . . . . . . . . . . . 191

Walter Fetter Lages

Online Trajectory Planning in ROS Under Kinodynamic

Constraints with Timed-Elastic-Bands . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231

Christoph Rösmann, Frank Hoffmann and Torsten Bertram

ix

x

Part III

Contents

Integration of ROS with Internet and Distributed Systems

ROSLink: Bridging ROS with the Internet-of-Things for Cloud

Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265

Anis Koubaa, Maram Alajlan and Basit Qureshi

ROS and Docker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285

Rufﬁn White and Henrik Christensen

A ROS Package for Dynamic Bandwidth Management

in Multi-robot Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 309

Ricardo Emerson Julio and Guilherme Sousa Bastos

Part IV

Service Robots and Fields Experimental

An Autonomous Companion UAV for the SpaceBot

Cup Competition 2015 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345

Christopher-Eyk Hrabia, Martin Berger, Axel Hessler,

Stephan Wypler, Jan Brehmer, Simon Matern and Sahin Albayrak

Development of an RFID Inventory Robot (AdvanRobot) . . . . . . . . . . . . 387

Marc Morenza-Cinos, Victor Casamayor-Pujol, Jordi Soler-Busquets,

José Luis Sanz, Roberto Guzmán and Rafael Pous

Robotnik—Professional Service Robotics Applications

with ROS (2) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 419

Roberto Guzmán, Román Navarro, Miquel Cantero

and Jorge Ariño

Using ROS in Multi-robot Systems: Experiences

and Lessons Learned from Real-World Field Tests . . . . . . . . . . . . . . . . . 449

Mario Garzón, João Valente, Juan Jesús Roldán,

David Garzón-Ramos, Jorge de León, Antonio Barrientos

and Jaime del Cerro

Part V

Perception and Sensing

Autonomous Navigation in a Warehouse

with a Cognitive Micro Aerial Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . 487

Marius Beul, Nicola Krombach, Matthias Nieuwenhuisen,

David Droeschel and Sven Behnke

Robots Perception Through 3D Point Cloud Sensors . . . . . . . . . . . . . . . . 525

Marco Antonio Simões Teixeira, Higor Barbosa Santos,

André Schneider de Oliveira, Lucia Valeria Arruda and Flavio Neves, Jr.

Contents

Part VI

xi

ROS Simulation Frameworks

Environment for the Dynamic Simulation of ROS-Based UAVs . . . . . . . 565

Alvaro Rogério Cantieri, André Schneider de Oliveira,

Marco Aurélio Wehrmeister, João Alberto Fabro

and Marlon de Oliveira Vaz

Building Software System and Simulation Environment for RoboCup

MSL Soccer Robots Based on ROS and Gazebo . . . . . . . . . . . . . . . . . . . 597

Junhao Xiao, Dan Xiong, Weijia Yao, Qinghua Yu, Huimin Lu

and Zhiqiang Zheng

VIKI—More Than a GUI for ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 633

Robin Hoogervorst, Cees Trouwborst, Alex Kamphuis

and Matteo Fumagalli

Editor and Contributors

About the Editor

Anis Koubaa is a full professor in Computer Science at Prince Sultan University and research

associate in CISTER Research Unit, ISEP-IPP, Portugal, add Senior Research Consultant with

Gaitech Robotics, China. He becomes a Senior Fellow of the Higher Education Academy

(SFHEA) in 2015. He received his B.Sc. in Telecommunications Engineering from Higher School

of Telecommunications (Tunisia), and M.Sc. degrees in Computer Science from University Henri

Poincare (France), in 2000 and 2001, respectively, and the Ph.D. degree in Computer Science

from the National Polytechnic Institute of Lorraine (France), in 2004. He was a faculty member at

Al-Imam University from 2006 to 2012. He has published over 120 refereed journal and conference papers. His research interest covers mobile robots, cloud robotics, robotics software

engineering, Internet-of-Things, cloud computing and wireless sensor networks. Dr. Anis received

the best research award from Al-Imam University in 2010, and the best paper award of the 19th

Euromicro Conference in Real-Time Systems (ECRTS) in 2007. He is the head of the ACM

Chapter in Prince Sultan University. His H-Index is 30.

Contributors

Maram Alajlan Center of Excellence Robotics and Internet of Things (RIOT)

Research Unit, Prince Sultan University, Riyadh, Saudi Arabia; King Saud

University, Riyadh, Saudi Arabia

Sahin Albayrak DAI-Labor, Technische Universität Berlin, Berlin, Germany

Kostas Alexis University of Nevada, Reno, NV, USA

Jorge Ariño Robotnik Automation, SLL, Ciutat de Barcelona, Paterna, Valencia,

Spain

Lucia Valeria Arruda Federal University of Technology—Parana, Curitiba,

Brazil

Nora Ayanian Department of Computer Science, University of Southern

California, Los Angeles, CA, USA

xiii

xiv

Editor and Contributors

Antonio Barrientos Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Guilherme Sousa Bastos System Engineering and Information Technology

Institute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

MG, Brazil

Sven Behnke Autonomous Intelligent Systems Group, University of Bonn, Bonn,

Germany

Martin Berger DAI-Labor, Technische Universität Berlin, Berlin, Germany

Torsten Bertram Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Marius Beul Autonomous Intelligent Systems Group, University of Bonn, Bonn,

Germany

Jan Brehmer DAI-Labor, Technische Universität Berlin, Berlin, Germany

Miquel Cantero Robotnik Automation, SLL, Ciutat de Barcelona, Paterna,

Valencia, Spain

Alvaro Rogério Cantieri Federal Institute of Parana, Curitiba, Brazil

Victor Casamayor-Pujol Universtitat Pompeu Fabra, Barcelona, Spain

Henrik Christensen Contextual Robotics Institute, University of California, San

Diego, CA, USA

Matthew Crosby Heriot-Watt University, Edinburgh, UK

Lúcia Valéria Ramos de Arruda Federal University of Technology—Parana,

Curitiba, Brazil

Jorge de León Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

André Schneider de Oliveira Advanced Laboratory of Embedded Systems and

Robotics (LASER), Federal University of Technology—Parana (UTFPR), Curitiba,

Brazil

Marlon de Oliveira Vaz Federal Institute of Parana, Curitiba, Brazil

Jaime del Cerro Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

David Droeschel Autonomous Intelligent Systems Group, University of Bonn,

Bonn, Germany

João Alberto Fabro Advanced Laboratory of Embedded Systems and Robotics

(LASER), Federal University of Technology—Parana (UTFPR), Curitiba, Brazil

Matteo Fumagalli Aalborg University, Copenhagen, Denmark

David Garzón-Ramos Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Editor and Contributors

xv

Mario Garzón Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

Bjarne Großmann Aalborg University Copenhagen, Copenhagen, Denmark

Roberto Guzmán Robotnik Automation S.L.L., Paterna, Valencia, Spain

Axel Hessler DAI-Labor, Technische Universität Berlin, Berlin, Germany

Frank Hoffmann Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Dirk Holz Bonn University, Bonn, Germany

Robin Hoogervorst University of Twente, Enschede, Netherlands

Christopher-Eyk Hrabia DAI-Labor, Technische Universität Berlin, Berlin,

Germany

Wolfgang Hönig Department of Computer Science, University of Southern

California, Los Angeles, CA, USA

Ricardo Emerson Julio System Engineering and Information Technology

Institute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

MG, Brazil

Mina Kamel Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Alex Kamphuis University of Twente, Enschede, Netherlands

Emanoel Koslosky Advanced Laboratory of Embedded Systems and Robotics

(LASER), Federal University of Technology—Parana (UTFPR), Curitiba, Brazil

Anis Koubaa Center of Excellence Robotics and Internet of Things (RIOT)

Research Unit, Prince Sultan University, Riyadh, Saudi Arabia; Gaitech Robotics,

Hong Kong, China; CISTER/INESC-TEC, ISEP, Polytechnic Institute of Porto,

Porto, Portugal

Nicola Krombach Autonomous Intelligent Systems Group, University of Bonn,

Bonn, Germany

Volker Krüger Aalborg University Copenhagen, Copenhagen, Denmark

Walter Fetter Lages Federal University of Rio Grande do Sul, Porto Alegre RS,

Brazil

Huimin Lu College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Simon Matern Technische Universität Berlin, Berlin, Germany

Marc Morenza-Cinos Universtitat Pompeu Fabra, Barcelona, Spain

Román Navarro Robotnik Automation, SLL, Ciutat de Barcelona, Paterna,

Valencia, Spain

xvi

Editor and Contributors

Flávio Neves Jr. Federal University of Technology—Parana, Curitiba, Brazil

Matthias Nieuwenhuisen Autonomous Intelligent Systems Group, University of

Bonn, Bonn, Germany

Ronald P.A. Petrick Heriot-Watt University, Edinburgh, UK

Athanasios

Denmark

S.

Polydoros Aalborg

University

Copenhagen,

Copenhagen,

Rafael Pous Universtitat Pompeu Fabra, Barcelona, Spain

Basit Qureshi Prince Sultan University, Riyadh, Saudi Arabia

Juan Jesús Roldán Centro De Automática y Robótica, UPM-CSIC, Madrid,

Spain

Francesco Rovida Aalborg University Copenhagen, Copenhagen, Denmark

Christoph Rösmann Institute of Control Theory and Systems Engineering, TU

Dortmund University, Dortmund, Germany

Higor Barbosa Santos Federal University of Technology—Parana, Curitiba,

Brazil

José Luis Sanz Keonn Technologies S.L., Barcelona, Spain

Roland Siegwart Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Jordi Soler-Busquets Universtitat Pompeu Fabra, Barcelona, Spain

Thomas Stastny Autonomous System Lab, ETH Zurich, Zurich, Switzerland

Marco Antonio Simões Teixeira Federal University of Technology—Parana,

Curitiba, Brazil

Cees Trouwborst University of Twente, Enschede, Netherlands

João Valente Centro De Automática y Robótica, UPM-CSIC, Madrid, Spain

Marco Aurélio Wehrmeister Advanced Laboratory of Embedded Systems and

Robotics (LASER), Federal University of Technology—Parana (UTFPR), Curitiba,

Brazil

Rufﬁn White Contextual Robotics Institute, University of California, San Diego,

CA, USA

Stephan Wypler Technische Universität Berlin, Berlin, Germany

Junhao Xiao College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Dan Xiong College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Editor and Contributors

xvii

Weijia Yao College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Qinghua Yu College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Zhiqiang Zheng College of Mechatronics and Automation, National University of

Defense Technology, Changsha, China

Part I

Control of UAVs

Model Predictive Control for Trajectory

Tracking of Unmanned Aerial Vehicles

Using Robot Operating System

Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

Abstract In this chapter, strategies for Model Predictive Control (MPC) design and

implementation for Unmaned Aerial Vehicles (UAVs) are discussed. This chapter is

divided into two main sections. In the first section, modelling, controller design and

implementation of MPC for multi-rotor systems is presented. In the second section,

we show modelling and controller design techniques for fixed-wing UAVs. System

identification techniques are used to derive an estimate of the system model, while

state of the art solvers are employed to solve the optimization problem online. By

the end of this chapter, the reader should be able to implement an MPC to achieve

trajectory tracking for both multi-rotor systems and fixed-wing UAVs.

1 Introduction

Aerial robots are gaining great attention recently as they have many advantages

over ground robots to execute inspection, search and rescue, surveillance and goods

delivery tasks. Depending on the task required to be executed, a multi-rotor system

or fixed-wing aircraft might be a more suitable choice. For instance, a fixed-wing

aircraft is more suitable for surveillance and large-scale mapping tasks thanks to

their long endurance capability and higher speed compared to a multi-rotor system,

while for an inspection task that requires flying close to structures to obtain detailed

footage a multi-rotor UAV is more appropriate.

M. Kamel (B) · T. Stastny · R. Siegwart

Autonomous System Lab, ETH Zurich, Zurich, Switzerland

e-mail: fmina@ethz.ch

T. Stastny

e-mail: tstastny@ethz.ch

R. Siegwart

e-mail: rsiegwart@ethz.ch

K. Alexis

University of Nevada, Reno, NV, USA

e-mail: kalexis@unr.edu

© Springer International Publishing AG 2017

A. Koubaa (ed.), Robot Operating System (ROS), Studies in Computational

Intelligence 707, DOI 10.1007/978-3-319-54927-9_1

3

4

M. Kamel et al.

Precise trajectory tracking is a demanding feature for aerial robots in general in

order to successfully perform required tasks, especially when operating in realistic

environments where external disturbances may heavily affect the flight performance

and when flying in the vicinity of structure. In this chapter, several model predictive

control strategies for trajectory tracking are presented for multi-rotor systems as well

as for fixed-wing aircraft. The general control structure followed by this chapter is a

cascade control approach, where a reliable and system-specific low-level controller

is present as inner loop, and a model-based trajectory tracking controller is running

as an outer loop. This approach is motivated by the fact that many critical flight

software is running on a separate navigation hardware which is typically based on

micro-controllers, such as Pixhawk PX4 and Navio [1, 2] while high level tasks are

running on more powerful -but less reliable- on-board computers. This introduces a

separation layer to keep critical tasks running despite any failure in the more complex

high-level computer.

By the end of this chapter, the reader should be able to implement and test various

Model Predictive Control strategies for aerial robots trajectory tracking, and integrate

these controllers into the Robot Operating System (ROS) [3]. Various implementation

hints and practical suggestions are provided in this chapter and we show several

experimental results to evaluate the proposed control algorithms on real systems.

In Sect. 2 the general theory behind MPC is presented, with focus on linear MPC

and robust linear MPC. In Sect. 3 we present the multi-rotor system model and

give hints on model identification approaches. Moreover, linear and robust MPC

are presented and we show how to integrate these controller into ROS and present

experimental validation results. In Sect. 4 we present a Nonlinear MPC approach for

lateral-directional position control of fixed-wing aircraft with implementation hints

and validation experiments.

2 Background

2.1 Concepts of Receding Horizon Control

Receding Horizon Control (RHC) corresponds to the historic evolution in control

theory that aimed to attack the known challenges of fixed horizon control. Fixed

horizon optimization computes a sequence of control actions {u 0 , u 1 , . . . , u N −1 }

over a horizon N and is characterized by two main drawbacks, namely: (a) when

an unexpected (unknown during the control design phase) disturbance takes place

or when the model employed for control synthesis behaves different than the actual

system, then the controller has no way to account for that over the computed control

sequence, and (b) as one approaches the final control steps (over the computer fixed

horizon) the control law “gives up trying” since there is too little time left in the fixed

horizon to go to achieve a significant objective function reduction. To address these

limitations, RHC proposed the alternative strategy of computing the full control

Model Predictive Control for Trajectory Tracking …

5

sequence, applying only the first step of it and then repeating the whole process

iteratively (receding horizon fashion). RHC strategies are in general applicable to

nonlinear dynamics of the form (considering that state update is available):

x˙ = f(x, u)

(1)

where the vector field f : Rn × Rm , x ∈ Rn×1 represents the state vector, and u ∈

Rm×1 the input vector. The general state feedback-based RHC optimization problem

takes the following form:

N −1

ref

min F(xt+N ) +

z

s.t.

||xt+k − xt+k || + ||ut+k ||

k=0

xt+k+1 = f (xt+k , ut+k )

ut+k ∈ UC

xt+k ∈ XC

xt = x(t)

(2)

where z = {ut , ut+1 , . . . , ut+N −1 } is the optimization variables, denotes some

(penalized) metric used for per-stage weighting, F(xt+N ) represents the terminal

ref

state weighting, xt+k is the reference signal, the subscript t + k is used to denote the

sample (using a fixed sampling time Ts ) of a signal at k steps ahead of the current

time t, while t + k + 1 indicates the next evolution of that, UC represents the set of

input constraints, XC the state constraints and x(t) is the value of the state vector at

the beginning of the current RHC iteration. The solution of this optimization problem

leads again to an optimal control sequence {ut , ut+1 , . . . , ut+N −1 } but only the first

step of that ut is applied while the whole process is then repeated iteratively.

Within this formulation, the term F(xt+N ) has a critical role for the closed-loop

stability. In particular, it forces the system state to take values within a particular set

at the end of the prediction horizon. It is relatively easy to prove stability per local

iteration using Lyapunov analysis. In its simplest case, this essentially means that

ref

considering the regulation problem (xt+k = 0 for k = 0, . . . , N − 1), and a “decrescent” metric , then the solution of the above optimization problem makes the system

stable at xt = 0, ut = 0 – that is that a terminal constraint xt+k = 0 is introduced (a

simplified illustration is provided in Fig. 1). However, the question of global stability

Fig. 1 Illustration of the terminal constraint set (Ω)

6

M. Kamel et al.

is in general not guaranteed. For that one has to consider the problem of introducing

both a terminal cost and a terminal constraint for the states [4]. However, general

constrained optimization problems can be extremely difficult to solve, and simply

adding terminal constraints may not be feasible. Note that in many practical cases,

the terminal constraint is not enforced during the control design procedure, but rather

verified a posteriori (by increasing the prediction horizon if not satisfied).

Furthermore, one of the most challenging properties of RHC is that of recursive feasibility. Unfortunately, although absolutely recommended from a theoretical

standpoint, it is not always possible to construct a RHC that has a-priori guarantee

of recursive feasibility, either due to theoretical or practical implications. In general,

a RHC strategy lacks recursive feasibility –and is therefore invalidated– even when

it is possible to find a state which is feasible, but where the optimal control action

moves the state vector to a point where the RHC optimization problem is infeasible.

Although a general feasibility analysis methodology is very challenging, for specific cases powerful tools exist. In particular, for the case of linear systems then the

Farkas’ Lemma [5] in combination with bilevel programming can be used to search

for problematic initial states which lack recursive feasibility – thus invalidating an

RHC strategy.

2.2 Linear Model Predictive Control

In this subsection we briefly present the theory behind MPC for linear systems. We

formulate the optimal control problem for linear systems with linear constraints in the

input and state variables. Moreover, we discuss the control input properties, stability

and feasibility in the case of linear and quadratic cost function. To achieve offset

free tracking under model mismatch, we adopt the approach described in [6] where

the system model is augmented with additional disturbances state d(t) to capture

the model mismatch. An observer is employed to estimate disturbances in steady

state. The observer design and the disturbance model will be briefly discussed in this

subsection.

J0 x0 , U, Xr e f , Ur e f

min

U

subject to xk+1 = Axk + Buk + Bd dk ;

dk+1 = dk , k = 0, . . . , N − 1

xk ∈ XC , uk ∈ UC

x N ∈ XC N

x0 = x (t0 ) , d0 = d (t0 ) .

(3)

The optimal control problem to achieve offset-free state tracking under linear

state and and input constraints is shown in (3), where J0 is the cost function, Xr e f =

ref

ref

{x0 , . . . , x N } is the reference state sequence, U = {u0 , . . . , u N −1 } and Ur e f =

ref

ref

{u0 , . . . , u N −1 } are respectively the control input sequence and the steady state

input sequence, Bd is the disturbance model and dk is the external disturbances, XC ,

Model Predictive Control for Trajectory Tracking …

7

UC and XC N are polyhedra. The choice of the disturbance model is not a trivial

task, and depends on the system under consideration and the type of disturbances

expected. The optimization problem is defined as

N −1

J0 x0 , U, X

ref

,U

ref

ref

ref

ref

ref

(xk − xk )T Qx (xk − xk )+

=

k=0

(uk − uk )T Ru (uk − uk )+

(4)

(uk − uk−1 ) RΔ (uk − uk−1 ) +

T

ref

ref

(x N − x N )T P(x N − x N ),

where Qx 0 is the penalty on the state error, Ru 0 is the penalty on control input

error, RΔ 0 is a penalty on the control change rate and P is the terminal state error

penalty.

In general, stability and feasibility of receding horizon problems are not ensured

except for particular cases such as infinite horizon control problems as in Linear

Quadratic Regulator (LQR) case. When the prediction horizon is limited to N steps,

the stability and feasibility guarantees are disputable. In principle, longer prediction

horizon tends to improve stability and feasibility properties of the controller, but the

computation effort will increase, and for aerial robot application, fast control action

needs to be computed on limited computation power platforms. However, the terminal

cost P and terminal constraint XC N can be chosen such that closed-loop stability and

feasibility are ensured [6]. In this chapter we focus more on the choice of terminal

weight P as it is easy to compute, while the terminal constraint is generally more

difficult and practically stability is achieved with long enough prediction horizon.

Note that in our cost function (4), we penalize the control input rate Δuk . This

ensures smooth control input and avoids undesired oscillations. In the cost function (4), u−1 is the actual control input applied on the system in the previous time

step.

As previously mentioned, offset-free reference tracking can be achieved by

augmenting the system model with disturbances dk to capture the modeling error.

Assuming that we want to track the system output yk = Cxk and achieve steady state

offset free tracking y∞ = r∞ . A simple observer that can estimate such disturbance

can be achieved as follows

xˆ k+1

dˆ k+1

=

A Bd

0 I

xˆ k

dˆ k

+

B

Lx

uk +

Ld

0

Cˆxk − ym,k

(5)

where xˆ and dˆ are the estimated state and external disturbances, Lx and Ld are the

observer gains and ym,k is the measured output at time k.

Under the assumption of stable observer, it is possible to compute the MPC state

at steady state xr e f and control input at steady state ur e f by solving the following

system of linear equations:

8

M. Kamel et al.

A−I B

C 0

xr e f,k

ur e f,k

=

−Bd dˆ k

rk

(6)

2.3 Nonlinear Model Predictive Control

Aerial vehicles behavior is better described by a set of nonlinear differential equations

to capture the aerodynamic and coupling effects. Therefore in this subsection we

present the theory behind Nonlinear MPC that exploits the full system dynamics,

and generally achieve better performance when it comes to aggressive trajectory

tracking. The optimization problem for nonlinear MPC is formulated in Eq. (7).

T

min

U

h (x(t), u(t)) − yr e f (t)

t=0

subject to

2

Q

dt + m (x(T )) − yr e f (T )

x˙ = f(x(t), u(t));

u(t) ∈ UC

x(t) ∈ XC

2

P

(7)

x(0) = x (t0 ) .

A direct multiple shooting technique is used to solve the Optimal Control Problem

(OCP) (7). In this approach the system dynamics are discretized over a time grid

t0 , . . . , t N within the time intervals tk , tk+1 . The inequality constraints and control

action are discretized over the same time grid. A Boundary Value Problem (BVP)

is solved for each interval and additional continuity constraints are imposed. Due

to the nature of the system dynamics and the imposed constraints, the optimization

problem becomes a Nonlinear Program (NLP). This NLP is solved using Sequential

Quadratic Programming (SQP) technique where the Quadratic Programs (QPs) are

solved by active set method using the qpOASES solver [7].

Note that, in case of infeasibility of the underlying QP, 1 penalized slack variables

are introduced to relax all constraints.

The controller is implemented in a receding horizon fashion where only the first

computed control action is applied to the system, and the rest of the predicted state

and control trajectory is used as initial guess for the OCP to solve in the next iteration.

2.4 Linear Robust Model Predictive Control

Despite the robustness properties of the nominal MPC formulation, specific robust

control variations exist when further robustness guarantees are required. The problem

of linear Robust Model Predictive Control (RMPC) may be formulated as a Minimax optimization problem that is solved explicitly. As an optimality metric we may

Model Predictive Control for Trajectory Tracking …

9

Relaxations - Derivation of Convex Optimization Problem

Feedback

Predictions

Objective Function

State Space

representation

using the concatenated vectors over

the prediction

horizon

Constraints Robustification

State and Input Constraints

Multiparametric

Optimizer

Explicit

Piecewise Affine

form

Extended

Sequential Table

Traversal

Fig. 2 Overview of the explicit RMPC optimization problem functional components

select the Minimum Peak Performance Measure (MPPM) for its known robustness

properties. Figure 2 outlines the relevant building blocks [8].

Within this RMPC approach, the following linear time invariant representation of

the system dynamics may be considered:

xk+1 = Axk + Buk + Gwk

yk+1 = Cxk

(8)

where xk ∈ X, uk ∈ U and the disturbing signals wk are unknown but bounded (wk ∈

W). Within this paper, box-constrained disturbances are considered (W∞ = {w :

||w||∞ ≤ 1}). Consequently, the RMPC problem will be formulated for the system

representation and additive disturbance presented above. Let the following denote

the concatenated versions of the predicted output, states, inputs and disturbances,

where [k + i|k] marks the values profile at time k + i, from time k.

T yT

T

Y = yk|k

k+1|k . . . yk+N −1|k

(9)

T xT

T

X = xk|k

k+1|k . . . xk+N −1|k

(10)

U=

T

uk|k

T

uk+1|k

W=

T

wk|k

T

wk+1|k

...

...

T

uk+N

−1|k

T

wk+N

−1|k

(11)

(12)

10

M. Kamel et al.

where X ∈ X N = X × X · · · × X, U ∈ U N = U × U · · · × U, W ∈ W N = W ×

W × · · · × W. The predicted states and outputs present linear dependency on the

current state, the future control input and the disturbance, and thus the following

holds:

X = Axk|k + BU + GW

Y = CX

(13)

where A, B , C , G are the stacked state vector matrices as in [8]. Subsequently, the

RMPC problem based on the MPPM (MPPM–RMPC) may be formulated as:

min max ||Y ||∞ , ||Y ||∞ = max ||yk+ j|k ||∞

u

w

j

(14)

s.t. uk+ j|k ∈ U, ∀ w ∈ W

xk+ j|k ∈ X, ∀ w ∈ W

wk+ j|k ∈ W

2.4.1

Feedback Predictions

Following the aforementioned formulation, the optimization problem will tend to

become conservative as the optimization essentially computes an open-loop control

sequence. Feedback predictions is a method to encode the knowledge that a receding

horizon approach is followed. Towards their incorporation, a type of feedback control

structure has to be assumed. Among the feedback parametrizations that are known

to lead to a convex problem space, the following is selected [9, 10]:

T υT

T

U = LW + V , V = υk|k

k+1|k · · · υk+N −1|k

⎛

⎞

0

0

0

···

0

⎜ L 10

0

0

···

0⎟

⎜

⎟

⎜ L 20

⎟

L

0

·

·

·

0

21

TL = ⎜

⎟

⎜

⎟

.

..

..

..

.

.

.

⎝

.

.⎠

.

.

.

L (N −1)0 L (N −1)1 · · · L (N −1)(N −2) 0

(15)

(16)

Employing this feedback predictions parameterization, the control sequence is now

parameterized directly in the uncertainty, and the matrix L describes how the control

action uses the disturbance vector. Inserting this parametrization yields the following

representation, where V becomes now the RMPC-manipulated action:

X = Axk|k + BV + (G + BL)W

U = LW + V

(17)

(18)

and the mapping from L, V to X , U is now bilinear. This allows the formulation of

the minimax MPC as a convex optimization problem [9]. Furthermore, let:

## c# 3.0 the complete reference (3rd edition)

## csharp 4.0 the complete reference

## Oracle Database 11g The Complete Reference P1

## Oracle Database 11g The Complete Reference P2

## Tài liệu SAP NetWeaver Portal Technology: The Complete Reference pptx

## Tài liệu HTML & CSS: The Complete Reference- P1 pdf

## Tài liệu HTML & CSS: The Complete Reference- P2 pdf

## Tài liệu HTML & CSS: The Complete Reference- P3 ppt

## Tài liệu HTML & CSS: The Complete Reference- P4 docx

## Tài liệu HTML & CSS: The Complete Reference- P5 doc

Tài liệu liên quan