Tải bản đầy đủ

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

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 fields 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, artificial 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, specifically 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 specific 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 affiliations.
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 Identification 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
Ruffin 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
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 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:


Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Tải bản đầy đủ ngay

×