Design and Experimental Validation of Tubebased MPC for Timedconstrained Robot Planning
Abstract
This paper deals with the design and experimental validation of a stateofthe art tubebased Model Predictive Control (MPC) for achieving timeconstrained tasks. Given the uncertain nonlinear dynamics of the robot as well as a highlevel task written in Metric Interval Temporal Logic (MITL), the goal is to design a feedback control law that guarantees the satisfaction of the task. The workspace is divided into Regions of Interest (RoI) and contains also unsafe regions (obstacles) that the robot should not visit. The feedback control law consists of two terms: a control input which is the outcome of a Finite Horizon Optimal Control (FHOCP); and a state feedback law that guarantees that the nominal trajectories are bounded within a tube centered along the nominal trajectories. The aforementioned control law guarantees that the robot is safely navigated through the RoI within certain time bounds. The proposed framework can handle the rich expressiveness of MITL and is experimentally tested with a Nexus mobile robot in our lab facilities. The experimental results show that the proposed framework is promising for solving reallife robotic as well as industrial problems.
I Introduction
Over the last years the field of controlling systems under formal verification constraints has been gaining significant research attention due to important applications in robotics, autonomous driving and industrial automation. The timed logic that has primarily been used is Metric Interval Temporal Logic [1, 2, 3, 4, 5, 6, 7]. The control synthesis under Metric Interval Temporal Logic (MITL) consists of three parts:

first, the dynamics of the robot are abstracted into a Weighted Transition System (WTS) by providing feedback control laws that can drive the robot between states; the time duration that the robot needs to navigate between the states is modeled as a weight to the transition system.

a product between the WTS and an automaton which accepts all words that satisfy the given formula is computed.

once a run is found in the product automaton, it maps back into a sequence of feedback laws that satisfy the given formula.
In practical applications, regarding the first part, the feedback control laws need to be appropriately designed such that the following specifications are taken into consideration: (a) state and input constraints; (b) obstacle avoidance; and (c) robustness against potential external disturbances and uncertainties.
A control technique that has been used for navigation of robotic agents with guaranteeing obstacle avoidance is the potential fields approach [8]. However, input constraints and potential external disturbances cannot be incorporated in the control design in a straightforward manner. In the same context, Prescribed Performance Controllers (PPC) that have been used for robotic navigation (see [9]) cannot handle input constraints as well as obstacle avoidance guarantees. Regarding realtime experiments, which is the main focus of the manuscript, both of the aforementioned methodologies require a significant amount of efforts in tuning the control gains. In particular, the potential fields approach requires computation of complicated formulas that consist of derivatives which might lead to numerical instabilities when applied to real platforms.
A feedback control law that has been recently proven to be efficient in incorporating the aforementioned specifications is the socalled tubebased Model Predictive Control (MPC) (see [10, 11, 12, 13, 14]). The idea of the tubebased MPC is based on the fact that the control law has two parts: a control law that is computed online and is the outcome of a FHOCP; and a statefeedback law designed offline and guarantees that the real system trajectories are always bounded in a tube whose volume depends on the bound of the disturbances as well as bounds of the derivatives of the dynamics. Experiments with MPC have been provided in [15, 16, 17, 18, 19, 20, 21, 22, 23] without incorporating any tube guarantees.
The contribution of this paper is to experimentally validate our recent theoretical results [2, 12, 13] in order to solve a timeconstrained planning problem for a Nexus mobile robot (see Fig. 1). In particular, inside of the workspace there exist a set of Regions of Interest (RoI) some of which may be unsafe regions (obstacles). Given an MITL formula that the Nexus is desired to satisfy, the three steps control synthesis MITL framework is demonstrated in practice by application of a sequence of tubebased MPC laws that guarantees the satisfaction of the formula. The experimental results of the paper in hand verify the efficiency of the proposed framework which solves a general category of timeconstrained robot navigation problems under state/input constraints, obstacle avoidance as well as uncertainties/disturbances. To the best of the authors’ knowledge, this is the first time that such a general control synthesis framework in terms of dynamics, language expressiveness and state/input constraints is demonstrated. The experimental results show that the proposed framework is promising for solving reallife robotic as well as industrial problems.
Ii Notation and Preliminaries
The fold Cartesian product of a set and its cardinality are defined by and , respectively; and , stand for the Euclidean and the weighted norm of a vector , respectively; stands for the minimum absolute value of the real part of the eigenvalues of ; and stand for the matrix with all entries zeros and the identity matrix, respectively; The set models a ball with center and radius , , respectively. Given the sets , and the matrix , the matrixset multiplication, the Minkowski addition and the Pontryagin difference are respectively defined by: , , and .
Definition 1.
[11] Consider a dynamical system where: , and . Consider a set . If there exists a feedback control law , such that for all and for all it holds that for all , along every solution , then is called a Robust Control Invariant (RCI) set for the system.
Definition 2.
[2] A Weighted Transition System (WTS) is a tuple where is a set of states; is a set of initial states; is a set of actions; is a transition relation; is a function that maps a weight to each transition; is a set of atomic propositions; and stands for the labeling function.
Definition 3.
[2] A timed run of a WTS is an infinite sequence , such that , and for all , it holds that and for a sequence of actions with , . The time stamps , are inductively defined as: ; , .
The Metric Interval Temporal Logic (MITL) [1] over a set of atomic propositions is defined by the grammar:
where , and , , and are the next, eventually, always and until temporal operator, respectively; , are the negation and conjunction operators, respectively; stands for a nonempty timed interval. For the semantics of MITL see [1]. Technical details regarding timed verification can be found in [24].
Iii Problem Formulation
Iiia System Model
Consider a robot operating in a bounded workspace with uncertain dynamics:
(1) 
where , are known and continuously differentiable functions; stands for the control input; and models the external disturbances and uncertainties. Consider also input constraints as well as bounded disturbances:
where , are a priori known. Define the corresponding nominal dynamics for (1) by:
which are the dynamics for the case of .
Assumption 1.
Let the dynamics:
be the Jacobian linearization of the nominal dynamics (1) around the equilibrium state . Then, we assume that the latter system is stabilizable.
Assumption 2.
There exists a strictly positive constant such that the following holds:
Furthermore, it holds that and .
In the given workspace, there exist Regions of Interest (RoI) labeled by . The RoI are modeled by the balls , , where and stands for the center and radius of RoI , respectively. At each time , the robot is occupying a ball that covers its volume, where and are its center and radius, respectively.
IiiB Objectives
The main goal of this paper is to design a feedback control law that steers the robot with dynamics as in (1) between RoI so that it obeys a highlevel task given in MITL. Define the labeling function:
(2) 
which maps each RoI with a subset of atomic propositions.
Definition 4.
A trajectory is uniquely associated with a timed run , if:

, i.e., the robot starts its motion at time ;

, for every ;

, i.e., initially, the volume of the robot is entirely within the RoI ;

, , i.e., the robot changes discrete state only when its entire volume is contained in the corresponding RoI;

, , where:
(3) is a function that models the time duration that the robot needs to be steered between regions and .
Definition 5.
A trajectory satisfies a formula written in MITL over a set of atomic propositions (written as , ), if and only if there exists a timed run to which the trajectory is uniquely associated, according to Definition 4, which satisfies .
Problem 1.
Consider a robot with dynamics as in (1), operating in a workspace . In the workspace, there exist RoI modeled by the balls , . Then, given an MITL formula over a set of atomic propositions and a labeling function as in (2), design a feedback control law which guarantees that:
according to Definition 5, while the robot remains in the workspace for all times.
Iv Problem Solution
Iva Feedback Control Design
Consider a robot with dynamics (1) occupying a RoI , at time . Denote by , the center of a desired RoI towards which the robot is required to be navigated. Define the error vector . Then, the uncertain error dynamics are given by:
(4) 
The corresponding nominal error dynamics are given by:
(5) 
Define the set that captures the state constraints by:
The first constraint denotes the fact that the robot should not intersect with any other RoI other than ,; the second one, denotes the fact that the robot needs to remain in the workspace for all times.
Define by the deviation between the real state of the system (4) and the nominal state of the system (5) with . The dynamics of the state are given by:
(6) 
where the function is defined by . Note that the following holds:
where and , are the Lipschitz constant of the functions , , respectively.
Lemma 1.
Proof.
Remark 1.
The volume of the tube that is centered along the nominal trajectories , depends on the upper bound of the disturbance , and the parameters , .
Hereafter we introduce the methodology under which the online control law is calculated. Denote by and the sampling step and the finite prediction horizon. Consider a sequence of sampling times , . Then, at every sampling time , the following FHOCP is solved:
(8a)  
subject to:  
(8b)  
(8c)  
(8d) 
where , and are positive definite gain matrices to be appropriately tuned. The state and input constraints sets are modified as:
in order to guarantee that while the FHOCP is solved for the nominal system dynamics (5), the real trajectories satisfy the state and input constraints , , respectively. The set , is used in order to ensure the nominal stability of the system [25].
Theorem 1.
According to the previous theorem, the robot with dynamics as in (1), starting its motion at time at RoI , under the control law (7), will have been navigated to RoI at time . For the computation of the time , Algorithm is used. In particular, the time that the robot is reaching the terminal set is captured by . This time models the time the the robot will have been navigated from RoI to , i.e, is consistent with (3).
IvB Discrete Abstractions and Control Synthesis
Definition 6.
The motion of the robot in the workspace is modeled through a WTS where:

is the set of states;

is the initial RoI that the robot starts its motion;

is set of actions containing the union of all feedback control laws of the form (7) that are able to drive the robot between RoI;

is the transition relation. We say that , , , , if there exist a feedback control that can drive the robot from RoI to the RoI ;
By using the framework depicted in Fig. 2, a sequence of feedback laws as in (7) that guarantee the satisfaction of the given MITL formula is designed. In particular, the dynamics of the robot (1) are abstracted into a WTS according to Definition 6; A product WTS is constructed by computing the product between and the Timed Büchi automaton , whose accepting runs are the ones that satisfy the formula . Then, by performing graph search in a timed run that maps into feedback control laws as in (7) can be computed. For the definitions of the product WTS and the Timed Büchi Automaton see [2].
V Experimental Setup and Results
This section demonstrates the efficacy of the proposed framework via a realtime experiment employing a Nexus mobile robot (Fig. 1). The experiment was carried out at Smart Mobility Lab (SML) (see Fig. 3 and [26]). The robot dimensions are mm and consists of aluminum mecanuum wheels which provide omnidirectional capabilities through the degrees of freedom (moving forward/backward, left/right and rotation). The rollers have a rotation of degrees with reference to the the plane of the wheel. Each wheel is connected to motors which they have optical encoders. The speed can be controlled by local PID controllers on an Arduino Controller and Arduino IO expansion board.
SML provides a motion capture system (MoCap) with Qualisys cameras spread across the lab. The MoCap provides the robot state vector, including pose, orientation as well as linear and angular velocities at frequency of Hz. The hardware used in experiments are connected using Gigabit Ethernet connections, USB connections, and wireless 5.8Ghz connections. The Raspberry Pi on the robot is connected with a TPLink Router via wireless Ghz connections. The host computer as well as the MoCap are connected with the TPLink Router via Gigabit Ethernet connections. The host computer runs the node of the controller, which receives the measurement from MoCap, and calculates the control signal.
The software implementation of the proposed control strategy was conducted in C++ under the Robot Operating System (ROS) [27]. Moreover, the Nonlinear Model Predictive Controller employed in this work is designed using the NLopt Optimization library [28] and runs on a desktop with cores, GHz Intel Core TM CPU and GB of RAM.
The workspace that the robot can operate in as well as a panoramic view of it are depicted in Fig. 3 and Fig. 4, respectively. It is captured by the set and contains RoI which are divided as follows:

the RoI , depicted with blue in Fig. 4; The RoI and map into the atomic propositions and , respectively.

the RoI , depicted with red in Fig. 4 which stand for unsafe regions that the robot needs to avoid. They map into the atomic propositions , and , respectively. Moreover, it holds that , .
The control input constraints are set to:
where , stand for the linear velocities and stands for the angular velocity. The desired MITL formula over the set of atomic propositions:
that the robot needs to satisfy is set to:
(9) 
The prediction horizon is chosen as . The gains are set to .
Video: A video demonstrating the experiment of this section can be found in the following link:
By employing Algorithm the transition times between the RoI that the robot is following are given as follows:
The evolution of the states , as well as the angle of the robot are presented in Fig. 5, Fig. 6 and Fig. 7, respectively. The control input signals , and are depicted in Fig. 8, Fig. 9 and Fig. 10, respectively. The transition times given above fulfill the constraints given by the MITL formula. Thus, it can be observed that the robot fulfills the MITL specification while all the imposed constraints from Problem 1 are satisfied.
Vi Conclusions
In this paper, we have experimentally validated recent theoretical results of robust nonlinear tubebased MPC along with timedconstrained highlevel planning. In particular, given the uncertain dynamics of a robot and a timed specification written in MITL, we have provided a framework under which a sequence of control laws under which the robot satisfies the desired task. The experimental platform consists of a Nexus mobile robot with an attached manipulator. The preliminary experimental results of the paper in hand verifies the efficiency of the proposed framework that solves a general category of timeconstrained robot navigation problems under state/input constraints, obstacle avoidance as well as uncertainties/disturbances. Future research involves extending the current framework towards the directions of:
References
 [1] R. Alur, T. Feder, and T. Henzinger, “The Benefits of Relaxing Punctuality,” Journal of the Association for Computing Machinery (JACM), vol. 43, no. 1, pp. 116–146, 1994.
 [2] A. Nikou, J. Tumova, and D. V. Dimarogonas, “Cooperative Task Planning of MultiAgent Systems Under Timed Temporal Specifications,” American Control Conference (ACC), pp. 13–19, 2016.
 [3] A. Nikou, D. Boskos, J. Tumova, and D. V. Dimarogonas, “Cooperative Planning Synthesis for Coupled MultiAgent Systems Under Timed Temporal Specifications,” American Control Conference (ACC), Seattle, USA, pp. 1847–1852, 2017.
 [4] S. Andersson, A. Nikou, and D. V. Dimarogonas, “Control Synthesis for MultiAgent Systems under Metric Interval Temporal Logic Specifications,” 20th World Congress of the International Federation of Automatic Control (IFAC WC), Toulouse, France, vol. 50, Issue 1, pp. 2397–2402, 2017.
 [5] A. Nikou, S. Heshmatialamdari, C. K. Verginis, and D. V. Dimarogonas, “Decentralized Abstractions and Timed Constrained Planning of a General Class of Coupled MultiAgent Systems,” 56th IEEE Conference on Decision and Control (CDC), pp. 990–995, Melbourne, Australia, 2017.
 [6] A. Nikou, “Cooperative Planning Control and Formation Control of MultiAgent Systems MultiAgent Systems,” Licentiate Thesis, KTH Royal Institute of Technology, Link: http://www.divaportal.org/smash/get/diva2:1094985/FULLTEXT01.pdf, 2017.
 [7] A. Nikou, D. Boskos, J. Tumova, and D. V. Dimarogonas, “On the Timed Temporal Logic Planning of Coupled MultiAgent Systems,” Automatica, vol. 97, pp. 339–345, 2018.
 [8] D. Koditschek and E. Rimon, “Robot Navigation Functions on Manifolds with Boundary,” Advances in Applied Mathematics, vol. 11, no. 4, pp. 412 – 442, 1990.
 [9] C. Bechlioulis and G. Rovithakis, “A LowComplexity Global ApproximationFree Control Scheme with Prescribed Performance for Unknown Pure Feedback Systems,” Automatica, vol. 50, no. 4, pp. 1217 – 1226, 2014.
 [10] W. Langson, I. Chryssochoos, S. Rakovic, and D. Mayne, “Robust Model Predictive Control Using Tubes,” Automatica, vol. 40, no. 1, pp. 125–133, 2004.
 [11] S. Yu, C. Maier, H. Chen, and F. Allgöwer, “Tube MPC Scheme Based on Robust Control Invariant Set with Application to Lipschitz Nonlinear Systems,” Systems and Control Letters, vol. 62, no. 2, pp. 194–200, 2013.
 [12] A. Nikou and D. V. Dimarogonas, “Robust Tubebased Model Predictive Control for Timedconstrained Robot Navigation,” American Control Conference (ACC), https://arxiv.org/pdf/1809.09825.pdf, Philadelphia, US, 2019.
 [13] A. Nikou and D. V. Dimarogonas, “Decentralized Tubebased Model Predictive Control of Uncertain Nonlinear MultiAgent Systems,” International Journal of Robust and Nonlinear Control (IJRNC), pp. 1–20, https://doi.org/10.1002/rnc.4522, 2019.
 [14] A. Nikou, C. K. Verginis, and D. V. Dimarogonas, “A Tubebased Nonlinear MPC Scheme for Interaction Control of Underwater Vehicle Manipulator Systems,” IEEE OES Autonomous Underwater Vehicle Symposium (AUVS), https://arxiv.org/pdf/1810.11789.pdf, Porto, Portugal, 2018.
 [15] A. Nikou, C. Verginis, S. Heshmatialamdari, and D. V. Dimarogonas, “A Nonlinear Model Predictive Control Scheme for Cooperative Manipulation with Singularity and Collision Avoidance,” IEEE Mediterranean Conference on Control and Automation (MED), Malta, pp. 707–712, 2017.
 [16] S. Heshmatialamdari, G. Karras, P. Marantos, and K. Kyriakopoulos, “A Robust Model Predictive Control Approach for Autonomous Underwater Vehicles Operating in a Constrained Workspace,” 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–5, 2018.
 [17] S. Heshmatialamdari, G. Karras, and K. Kyriakopoulos, “A Distributed Predictive Control Approach for Cooperative Manipulation of Multiple Underwater Vehicle Manipulator Systems,” 2019 IEEE International Conference on Robotics and Automation (ICRA), 2019.
 [18] S. Heshmatialamdari, G. Karras, A. Eqtami, and K. Kyriakopoulos, “A Robust Self Triggered Image Based Visual Servoing Model Predictive Control Scheme for Small Autonomous Robots,” 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 5492–5497, 2015.
 [19] S. Heshmatialamdari, A. Eqtami, G. Karras, D. V. Dimarogonas, and Kyriakopoulos, “A selftriggered visual servoing model predictive control scheme for underactuated underwater robotic vehicles,” 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 3826–3831, 2014.
 [20] S. Heshmatialamdari, A. Nikou, K. J. Kyriakopoulos, and D. V. Dimarogonas, “A Robust Force Control Approach for Underwater Vehicle Manipulator Systems,” 20th World Congress of the International Federation of Automatic Control (IFAC WC), Toulouse, France, vol. 50, Issue 1, pp. 11197–11202, 2017.
 [21] C. K. Verginis, A. Nikou, and D. V. Dimarogonas, “Communicationbased Decentralized Cooperative Object Transportation Using Nonlinear Model Predictive Control,” European Control Conference (ECC), pp. 733–738, Limassol, Cyprus, 2018.
 [22] A. Filotheou, A. Nikou, and D. V. Dimarogonas, “Decentralized Control of Uncertain MultiAgent Systems with Connectivity Maintenance and Collision Avoidance,” European Control Conference (ECC), pp. 8–13, Limassol, Cyprus, 2018.
 [23] A. Filotheou, A. Nikou, and D. V. Dimarogonas, “Robust Decentralized Navigation of MultiAgent Systems with Collision Avoidance and Connectivity Maintenance Using Model Predictive Controllers,” International Journal of Control (IJC), Available Online: https://www.tandfonline.com/doi/full/10.1080/00207179.2018.1514129, 2018.
 [24] P. Bouyer, “From Qualitative to Quantitative Analysis of Timed Systems,” Mémoire dâhabilitation, Université Paris, vol. 7, 2009.
 [25] H. Chen and F. Allgöwer, “A QuasiInfinite Horizon Nonlinear Model Predictive Control Scheme with Guaranteed Stability,” Automatica, vol. 34, no. 10, pp. 1205–1217, 1998.
 [26] “Smart mobility lab (SML),” https://www.kth.se/dcs/research/controloftransport/smartmobilitylab/smartmobilitylab1.441539.
 [27] M. Quigley, K. Conley, B. P. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “ROS: An OpenSource Robot Operating System,” International Conference on Robotics and Automation (ICRA) Workshop, 2009.
 [28] S. G. Johnson, “The NLopt NonlinearOptimization Package,” http://abinitio.mit.edu/nlopt, 2009.
 [29] C. K. Verginis, A. Nikou, and D. V. Dimarogonas, “Position and Orientation Based Formation Control of Multiple Rigid Bodies with Collision Avoidance and Connectivity Maintenance,” 56th IEEE Conference on Decision and Control (CDC), pp. 411–416, Melbourne, Australia, 2017.
 [30] A. Nikou, C. K. Verginis, and D. V. Dimarogonas, “Robust DistanceBased Formation Control of Multiple Rigid Bodies with Orientation Alignment,” 20th World Congress of the International Federation of Automatic Control (IFAC WC), Toulouse, France, pp. 15458–15463, 2017.
 [31] C. K. Verginis, A. Nikou, and D. V. Dimarogonas, “Robust Formation Control in SE(3) for TreeGraph Structures with Prescribed Transient and Steady State Performance,” Automatica, ArXiv Link: https://arxiv.org/abs/1803.07513, 2018.
 [32] A. Nikou, J. Tumova, and D. V. Dimarogonas, “Probabilistic Plan Synthesis for Coupled MultiAgent Systems,” 20th World Congress of the International Federation of Automatic Control (IFAC WC), Toulouse, France, vol. 50, Issue 1, pp. 10766–10771, 2017.
 [33] A. Nikou, G. Gavridis, and K. J. Kyriakopoulos, “Mechanical Design, Modeling and Control of a Novel Aerial Manipulator,” IEEE International Conference on Robotics and Automation (ICRA), Washington State Convention Center, Seattle, USA., pp. 4698–4703, 2015.
 [34] S. Heshmatialamdari, C. Bechlioulis, G. Karras, A. Nikou, D. V. Dimarogonas, and K. Kyriakopoulos, “A Robust Interaction Control Approach for Underwater Vehicle Manipulator Systems,” Annual Reviews in Control, vol. 46, pp. 315–325, 2018.