Robot operating system (ROS) the complete reference (volume 2) ( TQL )

Antonio Barrientos Centro De Automática y Robótica, UPM-CSIC, Madrid,SpainGuilherme Sousa Bastos System Engineering and Information TechnologyInstitute—IESTI, Federal University of Itaju

Studies in Computational Intelligence 707

Anis Koubaa Editor

Robot

Operating

System (ROS)The Complete Reference (Volume 2)

Volume 707

Series editor

Janusz Kacprzyk, Polish Academy of Sciences, Warsaw, Polande-mail: kacprzyk@ibspan.waw.pl

The series“Studies in Computational Intelligence” (SCI) publishes new ments and advances in the various areas of computational intelligence—quickly andwith a high quality The intent is to cover the theory, applications, and designmethods of computational intelligence, as embedded in the fields of engineering,computer science, physics and life sciences, as well as the methodologies behindthem The series contains monographs, lecture notes and edited volumes incomputational intelligence spanning the areas of neural networks, connectionistsystems, genetic algorithms, evolutionary computation, artificial intelligence,cellular automata, self-organizing systems, soft computing, fuzzy systems, andhybrid intelligent systems Of particular value to both the contributors and thereadership are the short publication timeframe and the worldwide distribution,which enable both wide and rapid dissemination of research output.

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

Robot Operating System

(ROS)

The Complete Reference (Volume 2)

Special focus on Unmanned Aerial Vehicles (UAVs) with ROS

123

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 fically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms 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 fic 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 filiations.

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

The Editor would like to thank the Robotics and Internet of Things (RIoT) Unit atCenter of Excellence of Prince Sultan University for their support to this work.Furthermore, the Editor thanks Gaitech Robotics in China for their support.

v

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

Research Unit, Portugal

Technologies)

TU Dortmund University

(continued)

vii

(continued)

Part I Control of UAVs

Model Predictive Control for Trajectory Tracking

of Unmanned Aerial Vehicles Using Robot Operating System 3Mina Kamel, Thomas Stastny, Kostas Alexis and Roland Siegwart

Designing Fuzzy Logic Controllers for ROS-Based Multirotors 41Emanoel Koslosky, André Schneider de Oliveira,

Marco Aurélio Wehrmeister and João Alberto Fabro

Flying Multiple UAVs Using ROS 83Wolfgang Hönig and Nora Ayanian

Part II Control of Mobile Robots

SkiROS—A Skill-Based Robot Control Platform on Top

of ROS 121Francesco 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 161Higor 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 Identification of the Dynamics of Mobile Robots

and Its Application to the Tuning of Controllers in ROS 191Walter Fetter Lages

Online Trajectory Planning in ROS Under Kinodynamic

Constraints with Timed-Elastic-Bands 231Christoph Rösmann, Frank Hoffmann and Torsten Bertram

ix

Part III Integration of ROS with Internet and Distributed Systems

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

Robotics 265Anis Koubaa, Maram Alajlan and Basit Qureshi

ROS and Docker 285Ruffin White and Henrik Christensen

A ROS Package for Dynamic Bandwidth Management

in Multi-robot Systems 309Ricardo Emerson Julio and Guilherme Sousa Bastos

Part IV Service Robots and Fields Experimental

An Autonomous Companion UAV for the SpaceBot

Cup Competition 2015 345Christopher-Eyk Hrabia, Martin Berger, Axel Hessler,

Stephan Wypler, Jan Brehmer, Simon Matern and Sahin Albayrak

Development of an RFID Inventory Robot (AdvanRobot) 387Marc 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) 419Roberto 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 449Mario 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 487Marius Beul, Nicola Krombach, Matthias Nieuwenhuisen,

David Droeschel and Sven Behnke

Robots Perception Through 3D Point Cloud Sensors 525Marco Antonio Simões Teixeira, Higor Barbosa Santos,

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

Part VI ROS Simulation Frameworks

Environment for the Dynamic Simulation of ROS-Based UAVs 565Alvaro 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 597Junhao Xiao, Dan Xiong, Weijia Yao, Qinghua Yu, Huimin Lu

and Zhiqiang Zheng

VIKI—More Than a GUI for ROS 633Robin Hoogervorst, Cees Trouwborst, Alex Kamphuis

and Matteo Fumagalli

About the Editor

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 con- ference 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 SaudUniversity, Riyadh, Saudi Arabia

Sahin Albayrak DAI-Labor, Technische Universität Berlin, Berlin, GermanyKostas 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 SouthernCalifornia, Los Angeles, CA, USA

xiii

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

Guilherme Sousa Bastos System Engineering and Information TechnologyInstitute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

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

Jan Brehmer DAI-Labor, Technische Universität Berlin, Berlin, GermanyMiquel 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, SanDiego, 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, SpainAndré Schneider de Oliveira Advanced Laboratory of Embedded Systems andRobotics (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, SpainDavid 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, BrazilMatteo Fumagalli Aalborg University, Copenhagen, Denmark

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

Mario Garzón Centro De Automática y Robótica, UPM-CSIC, Madrid, SpainBjarne Großmann Aalborg University Copenhagen, Copenhagen, DenmarkRoberto Guzmán Robotnik Automation S.L.L., Paterna, Valencia, SpainAxel Hessler DAI-Labor, Technische Universität Berlin, Berlin, GermanyFrank Hoffmann Institute of Control Theory and Systems Engineering, TUDortmund 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 SouthernCalifornia, Los Angeles, CA, USA

Ricardo Emerson Julio System Engineering and Information TechnologyInstitute—IESTI, Federal University of Itajubá—UNIFEI, Pinheirinho, Itajubá,

Nicola Krombach Autonomous Intelligent Systems Group, University of Bonn,Bonn, Germany

Volker Krüger Aalborg University Copenhagen, Copenhagen, DenmarkWalter Fetter Lages Federal University of Rio Grande do Sul, Porto Alegre RS,Brazil

Huimin Lu College of Mechatronics and Automation, National University ofDefense 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

Flávio Neves Jr Federal University of Technology—Parana, Curitiba, BrazilMatthias Nieuwenhuisen Autonomous Intelligent Systems Group, University ofBonn, Bonn, Germany

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

Athanasios S Polydoros Aalborg University Copenhagen, Copenhagen,Denmark

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, DenmarkChristoph Rösmann Institute of Control Theory and Systems Engineering, TUDortmund 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, SwitzerlandJordi Soler-Busquets Universtitat Pompeu Fabra, Barcelona, Spain

Thomas Stastny Autonomous System Lab, ETH Zurich, Zurich, SwitzerlandMarco 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, SpainMarco Aurélio Wehrmeister Advanced Laboratory of Embedded Systems andRobotics (LASER), Federal University of Technology—Parana (UTFPR), Curitiba,Brazil

Ruffin 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 ofDefense Technology, Changsha, China

Dan Xiong College of Mechatronics and Automation, National University ofDefense Technology, Changsha, China

Weijia Yao College of Mechatronics and Automation, National University ofDefense Technology, Changsha, China

Qinghua Yu College of Mechatronics and Automation, National University ofDefense Technology, Changsha, China

Zhiqiang Zheng College of Mechatronics and Automation, National University ofDefense Technology, Changsha, China

Control of UAVs

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 isdivided into two main sections In the first section, modelling, controller design andimplementation of MPC for multi-rotor systems is presented In the second section,

we show modelling and controller design techniques for fixed-wing UAVs Systemidentification techniques are used to derive an estimate of the system model, whilestate of the art solvers are employed to solve the optimization problem online Bythe end of this chapter, the reader should be able to implement an MPC to achievetrajectory tracking for both multi-rotor systems and fixed-wing UAVs

1 Introduction

Aerial robots are gaining great attention recently as they have many advantagesover ground robots to execute inspection, search and rescue, surveillance and goodsdelivery 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-wingaircraft is more suitable for surveillance and large-scale mapping tasks thanks totheir 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 detailedfootage a multi-rotor UAV is more appropriate

Autonomous System Lab, ETH Zurich, Zurich, Switzerland

© 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

Precise trajectory tracking is a demanding feature for aerial robots in general inorder to successfully perform required tasks, especially when operating in realisticenvironments where external disturbances may heavily affect the flight performanceand when flying in the vicinity of structure In this chapter, several model predictivecontrol 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 acascade 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 flightsoftware is running on a separate navigation hardware which is typically based onmicro-controllers, such as Pixhawk PX4 and Navio [1,2] while high level tasks arerunning on more powerful -but less reliable- on-board computers This introduces aseparation layer to keep critical tasks running despite any failure in the more complexhigh-level computer

By the end of this chapter, the reader should be able to implement and test variousModel Predictive Control strategies for aerial robots trajectory tracking, and integratethese controllers into the Robot Operating System (ROS) [3] Various implementationhints and practical suggestions are provided in this chapter and we show severalexperimental results to evaluate the proposed control algorithms on real systems

In Sect.2the general theory behind MPC is presented, with focus on linear MPCand robust linear MPC In Sect.3 we present the multi-rotor system model andgive hints on model identification approaches Moreover, linear and robust MPCare presented and we show how to integrate these controller into ROS and presentexperimental validation results In Sect.4we present a Nonlinear MPC approach forlateral-directional position control of fixed-wing aircraft with implementation hintsand validation experiments

2 Background

Receding Horizon Control (RHC) corresponds to the historic evolution in controltheory that aimed to attack the known challenges of fixed horizon control Fixedhorizon optimization computes a sequence of control actions {u0, u1, , 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 actualsystem, then the controller has no way to account for that over the computed controlsequence, and (b) as one approaches the final control steps (over the computer fixedhorizon) the control law “gives up trying” since there is too little time left in the fixedhorizon to go to achieve a significant objective function reduction To address theselimitations, RHC proposed the alternative strategy of computing the full control

sequence, applying only the first step of it and then repeating the whole processiteratively (receding horizon fashion) RHC strategies are in general applicable tononlinear dynamics of the form (considering that state update is available):

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

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

takes the following form:

the beginning of the current RHC iteration The solution of this optimization problemleads again to an optimal control sequence{u

t , u 

t+1, , u 

t +N−1} but only the first

step of that u t is applied while the whole process is then repeated iteratively

Within this formulation, the term F(x t +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 localiteration using Lyapunov analysis In its simplest case, this essentially means that

considering the regulation problem (xr e f t +k = 0 for k = 0, , N − 1), and a

“decres-cent” metric, then the solution of the above optimization problem makes the system

stable at xt = 0, u t = 0 – that is that a terminal constraint xt +k = 0 is introduced (asimplified illustration is provided in Fig.1) However, the question of global stability

Fig 1 Illustration of the terminal constraint set (Ω)

is in general not guaranteed For that one has to consider the problem of introducingboth a terminal cost and a terminal constraint for the states [4] However, generalconstrained optimization problems can be extremely difficult to solve, and simplyadding terminal constraints may not be feasible Note that in many practical cases,the terminal constraint is not enforced during the control design procedure, but ratherverified a posteriori (by increasing the prediction horizon if not satisfied).

Furthermore, one of the most challenging properties of RHC is that of sive feasibility Unfortunately, although absolutely recommended from a theoreticalstandpoint, it is not always possible to construct a RHC that has a-priori guarantee

recur-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 actionmoves the state vector to a point where the RHC optimization problem is infeasible.Although a general feasibility analysis methodology is very challenging, for spe-cific cases powerful tools exist In particular, for the case of linear systems then theFarkas’ Lemma [5] in combination with bilevel programming can be used to searchfor problematic initial states which lack recursive feasibility – thus invalidating anRHC strategy

In this subsection we briefly present the theory behind MPC for linear systems Weformulate the optimal control problem for linear systems with linear constraints in theinput and state variables Moreover, we discuss the control input properties, stabilityand feasibility in the case of linear and quadratic cost function To achieve offsetfree 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 steadystate The observer design and the disturbance model will be briefly discussed in thissubsection

N−1} are respectively the control input sequence and the steady state

input sequence, B is the disturbance model and d is the external disturbances,X ,

U C andX C N are polyhedra The choice of the disturbance model is not a trivialtask, and depends on the system under consideration and the type of disturbancesexpected The optimization problem is defined as

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 ensuredexcept 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 predictionhorizon tends to improve stability and feasibility properties of the controller, but thecomputation effort will increase, and for aerial robot application, fast control actionneeds to be computed on limited computation power platforms However, the terminal

cost P and terminal constraintX C Ncan be chosen such that closed-loop stability andfeasibility 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Δu k Thisensures smooth control input and avoids undesired oscillations In the cost func-tion (4), u−1 is the actual control input applied on the system in the previous timestep

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 = Cxkand achieve steady state

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

can be achieved as follows



B 0

where ˆx and ˆd are the estimated state and external disturbances, Lxand 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 followingsystem of linear equations:

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

to capture the aerodynamic and coupling effects Therefore in this subsection wepresent the theory behind Nonlinear MPC that exploits the full system dynamics,and generally achieve better performance when it comes to aggressive trajectorytracking The optimization problem for nonlinear MPC is formulated in Eq (7)

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

to the nature of the system dynamics and the imposed constraints, the optimizationproblem becomes a Nonlinear Program (NLP) This NLP is solved using SequentialQuadratic Programming (SQP) technique where the Quadratic Programs (QPs) aresolved 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 firstcomputed control action is applied to the system, and the rest of the predicted stateand control trajectory is used as initial guess for the OCP to solve in the next iteration

Despite the robustness properties of the nominal MPC formulation, specific robustcontrol variations exist when further robustness guarantees are required The problem

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

Mini-State Space representation using the concate- nated vectors over the prediction horizon Feedback Predictions

State and Input Constraints

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 robustnessproperties Figure2outlines the relevant building blocks [8]

Within this RMPC approach, the following linear time invariant representation ofthe system dynamics may be considered:

yk+1 = Cxk

where xk ∈ X, u k ∈ U and the disturbing signals wkare 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 systemrepresentation and additive disturbance presented above Let the following denotethe 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.

where X ∈ XN = X × X · · · × X, U ∈ UN = U × U · · · × U, W ∈ WN = 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 followingholds:

Following the aforementioned formulation, the optimization problem will tend tobecome conservative as the optimization essentially computes an open-loop controlsequence Feedback predictions is a method to encode the knowledge that a recedinghorizon approach is followed Towards their incorporation, a type of feedback controlstructure has to be assumed Among the feedback parametrizations that are known

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

X =Ax k |k+BV + (G+BL)W (17)

and the mapping fromL, VtoX , Uis now bilinear This allows the formulation ofthe minimax MPC as a convex optimization problem [9] Furthermore, let:

(20)

denote the concatenated –over the prediction horizon– versions of the input and

state constraints f u and f x More specifically, f u = [ f u (1)max, f u (1)min .] and

f x = [ f x (1)max, f x (1)min .] where f u (i)max, f u (i)min represent the maximum and

minimum allowed input values of the i -th input, while f x ( j)max, f x ( j)minrepresent

the maximum and minimum acceptable/safe state configurations of the j -th state.

defined as a positive scalar value that bounds (from above) the objective function.The peak constraint may be equivalently reformulated as:

C(A x k |k+BV ) + C(G+BL)W ≤ τ1, ∀ W ∈ WN

(22)

−C(Axk |k+BV ) − C(G+BL)W ≤ τ1, ∀ W ∈ WN

(23)

where 1 is a vector of ones(1 1 · · · 1) T

with suitable dimensions Satisfaction ofthese uncertain inequalities is based on robust optimization methods

2.4.2 Robust Uncertain Inequalities Satisfaction

Since box-constrained disturbances (w ∈ W∞) are assumed, the following holds:

max

|x|≤1 c

T

This equation holds as max|x|≤1 c T x= max|x|≤1c i x i =c isign(ci ) = ||c||1

Consequently, the uncertain constraints with w∈ W∞are satisfied as long as [9]:

C (A x k |k+BV) + |C(G+BL)|1 ≤ τ1 (25)

−C(A x k |k+BV) + |C(G+BL)|1 ≤ τ1 (26)

To handle these constraints in a linear programming fashion [5], the term|C(G+

BL)| is bounded from above by introducing a matrix variable Γ  0:

2.4.3 Robust State and Input Constraints

To robustly satisfy hard constraints on the input and the states along the predictionhorizon, a new matrixΩ  0 is introduced and the constraints are reformulated as:

con-2.4.4 Minimum Peak Performance Robust MPC Formulation

Based on the aforementioned derivations, the total MPPM–RMPC formulation

is solved subject to element-wise bounded disturbances and feedback predictionsthrough the following linear programming problem:

2.4.5 Multiparametric Explicit Solution

The presented RMPC strategy requires the solution of a linear programming problem.However, a multiparametric-explicit solution is possible due to the fact that thecontrol action takes the general form [6]:

whereΠ i

, r = 1, , N r

are the regions of the receding horizon controller The r -th

control law is valid if the state vector xkis contained in a convex polyhedral region

Π r = {xk|Hrxk≤ Kr } computed and described in h-representation [11] Such a factenables fast real-time execution even in microcontrollers with very limited computingpower In this framework, the real-time code described in [8] is employed

3 Model-Based Trajectory Tracking Controller for

Multi-rotor System

In this section, we present a simplified model of multi-rotor system that can be usedfor model-based control to achieve trajectory tracking, and we present a linear andnonlinear model predictive controller for trajectory tracking

The 6DoF pose of the multi-rotor system can be defined by assigning a fixed inertial

frame W and body frame B attached to the vehicle as shown in Fig.3 We denote by

p the position of the origin of frame B in frame W expressed in frame W, by R the rotation matrix of frame B in frame W expressed in frame W Moreover, we denote

byφ, θ and ψ the roll, pitch and yaw angles of the vehicle In this model we assume

a low level attitude controller that is able to track desired roll and pitchφ d , θ dangleswith a first order behavior The first order inner-loop approximation provides suffi-cient information to the MPC to take into account the low level controller behavior.The inner-loop first order parameters can be identified through classic system identi-fication techniques The non-linear model used for trajectory tracking of multi-rotorsystem is shown in Eq (37)

Fig 3 Illustration of the Firefly hexacopter from Ascending Technologies with attached body fixed

frame B and inertial frame W

where v indicates the vehicle velocity, g is the gravitational acceleration, T is the

mass normalized thrust, A x , A y , A zindicate the mass normalized drag coefficients,

d is external disturbance.τ φ , K φandτ θ , K θare the time constant and gain of loop behavior for roll angle and pitch angle respectively

inner-The cascade controller structure assumed in this chapter is shown in Fig.4

In this subsection we show how to formulate a linear MPC to achieve trajectorytracking for multi-rotor system and integrate it into ROS The optimization problempresented in Eq (3) is solved by generating a C-code solver using the CVXGENframework [12] CVXGEN generates a high speed solver for convex optimizationproblems by exploiting the problem structure For clarity purposes, we rewrite theoptimization problem here and show how to generate a custom solver using CVX-GEN The optimization problem is given by

Fig 4 Controller scheme for multi-rotor system n1 n m are the i − th rotor speed and y is the

measured vehicle state

discrete system model A, B, Bd.

3.2.1 Attitude Loop Parameters Identification

The attitude loop identification is a recommended process when no knowledge isavailable about the attitude controller used onboard of the vehicle This is the casefor many commercially available platforms To perform this system identification,typically a pilot excites the vehicles axes in free flight Attitude command andactual vehicle attitude (estimated using motion capture system if available or InertialMeasurement Unit (IMU)) are logged with accurate time stamp Typically twodatasets are collected, one is used for parameters estimation and the other dataset

is used for validation purpose We will not go into details of system identification,interested readers can find more details in [13] A set of scripts for attitude dynamicsidentification will be made open source upon acceptance of the chapter

To perform parameters identification using the available scripts please follow thefollowing steps:

1 Prepare the system and make sure you are able to log time stamped attitudecommands and actual vehicle attitude

2 Perform a flight logging the commands and vehicle attitude in a bag file Duringthe flight excite as much as possible each axis of the vehicle

3 Repeat the flight test to collect validation dataset

4 Set the correct bag files name and topics name in the provided script and run it

5 The controller parameters will be displayed on screen with validation percentage

to confirm the validity of the identification

3.2.2 Linearization, Decoupling and Discretization

For controller design, the vehicle dynamics can be approximated around hoveringcondition where small attitude angles are assumed and vehicle heading aligned with

inertial frame x axis (i.e.ψ = 0) The linearized system around hovering condition

where the state vector is x=(p T , v T ,Wφ,Wθ) T, the input vector u= (Wφ d ,Wθ d , T ) T

and the disturbance vector d= (d x , d y , d z ) T The c subscription indicates that this

is a continuous time model Note that in this linearization we marked the attitude to

be in inertial frame W to get rid of the yaw angleψ from the model The attitude

control actionWφ d ,Wθ d is computed in inertial frame and then converted to body

frame by performing a rotation around z axis The control action in the vehicle body

Given that the controller is implemented in discrete time, it is necessary to cretize the system dynamics (39) This can be done as follows

where T sis the sampling time The computation of the terminal cost P matrix is done

by solving the Algebraic Riccati Equation iteratively

3.2.3 ROS Integration

The strategy followed for the ROS integration of the solver is to create a ROS node tointerface the controller to ROS environment, while the controller, estimator and othercomponents are implemented as C++ shared libraries that get linked to the node atcompilation time The controller node expects the vehicle state as a nav odometrymessage and publishes a custom message of type RollPitchYawRateThrustcommand message The desired trajectory can be sent to the controller over a topic oftype MultiDOFJointTrajectory or as single desired point over a topic of typePoseStamped The advantage of passing the whole desired trajectory over singlepoint is that the MPC can take into consideration the future desired trajectory andreact accordingly Figure5gives an overview of nodes and topics communicationthrough a ros graph diagram

Fig 5 ROS graph diagram showing various nodes and topics to control multi-rotor system

Each time the controller receives a new odometry message, a control action iscomputed and published Therefore it is important to guarantee that the state estimator

is publishing an odometry message at the desired rate of control We recommend touse this controller with the Modular Framework for MultiSensor Fusion [14] Animportant point to consider when implementing such a controller is to reduce asmuch as possible the delays in the loop A hint to minimize communication delay

is to use the ROS transportation hints by passing ros::TransportHints().tcpNoDelay()flag to the odometry subscriber

The controller node publishes the following information:

1 Current desired reference as tf

Fig 6 Through dynamic reconfiguration it is possible to change the controller parameters online

for tuning purposes

2 Desired trajectory as rviz marker.

3 Predicted vehicle state as rviz marker

The controller parameters can be easily changed in run time from the dynamic figure as shown in Fig.6

recon-An open source implementation of the presented controller can be found inhttps://github.com/ethz-asl/mav_control_rw

-3.5 -3 -2.5

y [m]

-2 -1.5 0

(a) Aggressive trajectory tracking using

linear MPC.3D trajectory plot

time [s]

-3 -2 -1 0 1 2 3

Position tracking performance

x z

(c) Velocity tracking performance using linear MPC

Fig 7 Aggressive trajectory tracking performance using linear MPC controller running onboard

of Firefly hexacopter from Ascending Technologies

3.2.4 Experimental Results

To validate the controller performance, we track an aggressive trajectory Thecontroller is running onboard a Firefly hexacopter from Ascending Technologies(AscTec) with a NUC i7 computer onboard An external motion capture systemVicon [15] is used as pose sensor and fused with onboard IMU using the MultisensorFusion Framework (MSF) The controller is running at 100 Hz and the predictionhorizon is chosen to be 20 steps

Figure7shows the tracking performance of the controller

In this subsection we use the full vehicle nonlinear model to design a continuoustime nonlinear model predictive controller The toolkit employed to generate thenonlinear solver is ACADO [16] which is able to generate very fast custom C codesolvers for general optimal control problems OCP The optimization problem can bewritten as

3.3.2 Experimental Results

The same setup used for the linear MPC is used to evaluate the nonlinear MPC Theonly difference is that we are currently evaluating the controller on a more aggressivetrajectory as shown in Fig.8

3.3.3 Robust Linear Model Predictive Control for Multirotor System

In order to evaluate the proposed RMPC, a set of test-cases were conducted using thestructurally modified AscTec Hummingbird quadrotor (ASLquad) shown in Fig.9.For the implementation of the RMPC, a software framework around ROS was devel-oped In particular, a set of low-level drivers and nodes handle the communicationwith the attitude and motor control on-board the aerial robot and therefore enabling

us to provide attitude and thrust references through the RMPC MATLAB was used

to derive and compute the explicity formulation of the RMPC, while the completeexplicit algorithm overviewed in Sect.2.4.5was implemented within a SIMULINKblock Auto-code generation was then employed to extract the C-code equivalent

(c) Sequence of rviz screen shot showing the reference trajectory in

green and predicted vehicle state in red

Fig 8 Aggressive trajectory tracking performance using nonlinear MPC controller running

onboard of Firefly hexacopter from Ascending Technologies

of this controller, which was then wrapped around a ROS node and integrated intothe overall software framework For the described experiments with the RMPC, fullstate feedback is provided through external motion capture, while an alignment step

to account for relative orientation of the on-board attitude and heading estimationsystem also takes place

Below we present the results on (a) trajectory tracking subject to wind bances, and (b) slung load operations, while further results are available at [8] For

distur-Fig 9 Instance of an RMPC test for slung load operations

the presented experiments, the sampling time was set to T s= 0.08s, the

predic-tion horizon was set to N = 6 for both of them, while the following state and inputconstraints were considered:

1.5 m/s

1.5 m/s

1/5 m/s π/4 rad π/4 rad π/4 rad π/4 rad

1.5 m/s

1.5 m/s

1/5 m/s π/4 rad π/6 rad π/4 rad

Consequently, the capabilities of performing slung load operations were ered As shown in Fig.11where a forcible external disturbance is also applied ontothe slung load (a hit on the load), highly precise position hold and disturbance rejec-tion results were achieved This is despite the fact that the slung load introducesdisturbances that are phase-delayed compared to the vehicle states and the controller

consid-is not augmented regarding its state to incorporate the load’s motion For thconsid-is