A Robust Nonlinear MPC Framework for Control of Underwater Vehicle Manipulator Systems under High-Level Tasks

: Over the last years, the development and control of Autonomous Underwater Vehicles (AUV) with attached robotic manipulators, also called Underwater Vehicle Manipulator System (UVMS), has gained signiﬁcant research attention. In such applications, feedback controllers which guarantee that the end-effector of the UVMS is fulﬁlling desired complex tasks should be designed in a way that state and input constraints are taken into consideration. Furthermore, due to their complicated structure, unmodeled dynamics as well as external disturbances may arise. Complex tasks can be conveniently given in the so-called Linear Temporal Logic (LTL). Motivated by this, we develop a combined abstraction and control synthesis framework in which, given the uncertain kinematics/dynamics of the UVMS, a workspace divided into Regions of Interest (RoI) and a desired LTL task, a sequence of feedback control laws that provably guarantee the LTL formula is provided. The feedback control law consists of two parts: an on-line controller which is the outcome of a Finite Horizon Optimal Control Problem (FHOCP); and a backstepping feedback law which is tuned off-line and guarantees that the real trajectory always remains in a bounded hyper-tube centered along the nominal trajectory of the end-effector. The proposed controller falls within the tube-based Nonlinear Model Predictive Control (NMPC) methodology and can handle the rich expressivity of LTL in both safety and reachability speciﬁcations. Numerical simulations conducted in MATLAB verify the validity of the proposed framework.


Introduction
Most of the autonomous underwater manipulation tasks, such as maintenance of ships, underwater weld inspection, surveying oil/gas searching, require the UVMS to fulfill a sequence of high-level tasks imposed by the user (see [1][2][3][4][5][6][7][8][9]).An example of a high-level task could be "Periodically inspect the ship while avoiding collision with it and with environment".The high-level tasks can be conveniently expressed in temporal-logic languages.Temporal-logic based task planning has gained a significant amount of recent attention, as it provides a fully automated correct-by-design controller synthesis approach for autonomous robots.Moreover, it provides formal high-level languages that can describe planning objectives which are more complex than the well-studied navigation or stabilization tasks [10][11][12].
The qualitative specification language that has been used to express the high-level tasks is Linear Temporal Logic (LTL) [10].Given the dynamics of an autonomous robot and a desired LTL formula, the controller synthesis procedure is given as follows [13]: first, the robot dynamics are abstracted into a discrete representation, the so-called Transition System (TS); second, a product between the TS and an automaton that accepts the runs that satisfy the given LTL formula is computed; and third, once an accepting run in the product is found, it maps into a sequence of feedback controllers of the robot dynamics.
In this paper we aim to employ the aforementioned procedure in order to design feedback control laws which guarantee that a UVMS satisfies a desired LTL specification.Regarding underwater applications, the most challenging part of the LTL control synthesis procedure is the abstraction part due to the fact that the continuoustime controller needs to take into account the following issues: • the highly nonlinear and complicated dynamics of the UVMS; • state and input constraints; • external disturbances, uncertainties as well as unmodeled dynamics; • the unstructured underwater environment which contains different types of obstacles.
At the same time, regarding practical applications, the UVMS needs to visit specific parts of the workspace, which are called Regions of Interest (RoI)), and execute desired tasks through its end-effector.An example of this procedure is the inspection of ship for damages by a UVMS, in which different parts of the ship are different RoI that the UVMS is required to visit and execute desired actions.
Motivated by the above, in this paper we provide a feedback control design framework that takes into account the aforementioned issues.In particular, we provide: • a nonlinear robust NMPC framework that deals with the model uncertainties, external disturbances as well as control input constraints; • a three-stage procedure that provides a planning framework that guarantees the satisfaction of the desired high-level task.
One of the main challenges in Nonlinear Model Predictive Control framework [14][15][16][17] is the efficient handling of external disturbances/uncertainties.A promising robust NMPC strategy, originally proposed for discrete-time linear systems in [18] and studied for nonlinear systems in [19][20][21], is the so called tube-based approach.Then, starting by the kinematic and dynamic modeling of a general uncertain UVMS equipped with an n Degrees of Freedom (DoF) manipulator, given a partitioned workspace into RoI and a desired task written in LTL, a systematic control design methodology for tube-based NMPC which guarantees robust transition between RoI under safety constraints is developed; in particular, the controller consists of two terms: a nominal control input, which is computed on-line and is the outcome of a Finite Horizon Optimal Control Problem (FHOCP) that is repeatedly solved at every sampling time instant, for its nominal system dynamics; and an additive state feedback law which is designed by a backstepping nonlinear control procedure and guarantees that the real trajectory of the closed-loop system will belong to a hyper-tube centered along the nominal trajectory.The volume of the hyper-tube depends on the upper bound of the disturbances, the bounds of the Jacobian matrix as well as the Lipschitz constants of the UVMS dynamics.Secondly, we exploit the aforementioned control design in order to abstract the dynamics of the robot into a TS.Then, by performing the LTL control synthesis procedure, a sequence of control laws that guarantees the satisfaction of the formula is provided.
A preliminary conference version of this paper can be found in [22], in which a robust force/torque NMPC scheme for a UVMS in contact with a compliant surface of the environment is designed.That framework has not considered any high-level tasks and safety with potential obstacles of the environment.
The rest of the paper is structured as follows: Section 2 provides the notation that will be used as well as necessary background knowledge; in Section 3, the problem treated in this paper is formally defined; Section 4 contains the main results of the paper; Section 5 is devoted to a simulation example; and in Section 6, conclusions and future research directions are discussed.

Notation and Preliminaries
Some mathematical notations and preliminaries to be used throughout this paper are given below.Define by N and R the sets of positive integers and real numbers, respectively.Given a set S, denote by |S| its cardinality, by S n := S × • • • × S its n-fold Cartesian product and by 2 S the set of all its subsets.Given a vector z ∈ R n define by: its Euclidean and weighted norm, with P ≥ 0. Given vectors z 1 , z 2 ∈ R 3 , S : R 3 → so(3) stands for the skew-symmetric matrix defined according to S(z 1 )z 2 = z 1 × z 2 where: The notation λmin(P ) stands for the minimum absolute value of the real part of the eigenvalues of P ∈ R n×n ; 0 m×n ∈ R m×n and In ∈ R n×n stand for the m × n matrix with all entries zeros and the identity matrix, respectively.The notation diag{P 1 , . . ., Pn} stands for the block diagonal matrix with the matrices P 1 , . . ., Pn in the main diagonal; stands for a ball in R 3 with center and radius χ ∈ R 3 , r > 0, respectively.Given coordination frames Σ i , Σ j , denote by R j i the transformation from Σ i to Σ j .Given sets S 1 , S 2 ⊆ R n , S 3 ⊆ R m and the matrix P ∈ R n×m , the Minkowski addition, the Pontryagin difference and the matrix-set multiplication are respectively defined by: [21] For any constant ρ > 0, vectors z 1 , z 2 ∈ R n and matrix P ∈ R n×n , P > 0 it holds that: 1) for a fixed s, the mapping β(r, s) belongs to class K with respect to r; 2) for a fixed r, the mapping β(r, s) is strictly decreasing with respect to s; and it holds that lim s→∞ β(r, s) = 0.

Definition 2.
[19] Consider a dynamical system: with initial condition χ(0) ∈ X .A set X ⊆ X is a Robust Control Invariant (RCI) set for the system, if there exists a feedback control law u := κ(χ) ∈ U, such that for all χ(0) ∈ X and for all disturbances d ∈ D it holds that χ(t) ∈ X for all t ≥ 0, along every solution χ(t).
Definition 3. A nonlinear system ẋ = f (x, u, d), x ∈ X , u ∈ U, d ∈ D, with initial condition x(0) ∈ X is said to be Input-to-State Stable (ISS) with respect to d ∈ D, if there exist functions β ∈ KL, γ ∈ K such that for any initial condition x(0) ∈ X and for any input u(t) ∈ U , the solution x(t) exists for all t ∈ R ≥0 and satisfies: An atomic proposition is a statement that is either true or false.
Definition 4. A Transition System (TS) is a tuple (S, S 0 , Act, −→ , Π, L) where: • S is a finite set of states; • S 0 ⊆ S is a set of initial states; • Act is a set of actions (control inputs); • −→⊆ S × Act × S is a transition relation; • Π is a finite set of atomic propositions; • L : S → 2 Π is a labeling function function that assigns to each state the atomic propositions that are true in that state.
For a given state s ∈ S, define by Post(s, α) := {s ∈ S : (s, α, s ) ∈−→} the set of successors of the state s with action α.An infinite sequence of states of the form r = s 0 s 1 s 2 . . . is called a run of the TS if s 0 ∈ S 0 and s i+1 ∈ Post(s i , •) for all i ≥ 0. The trace of the run r = s 0 s 1 s 2 . . . is defined by: Trace(r) := L(s 0 )L(s 1 )L(s 2 ) . . ., In this paper we focus on task specifications ϕ given in Linear Temporal Logic (LTL).The syntax of LTL (see [10]) over a set of atomic propositions Π is defined by the grammar: where ∈ Π and , U stand for the next and until operators, respectively; ¬ and ∧ are the negation and conjunction operator respectively.The always ( ) and eventually (♦) operators can be defined by := ¬♦¬ϕ and ♦ := Uϕ, respectively.LTL formulas are interpreted on infinite words r = w 0 w 1 w 2 . . .where w i ∈ 2 Π , ∀i ≥ 0. The satisfaction relation is denoted by |=, i.e., for a word w and an LTL formula ϕ, we write w |= ϕ iff w satisfies ϕ.The satisfaction relation is defined inductively as follows: Given a TS, its infinite run r = r 0 r 1 r 2 . . .and an LTL formula ϕ over Π, we say that r satisfies ϕ, denoted by r |= ϕ, if A infinite run q 0 q 1 q 2 . . . in the NBA is called accepting if there exist infinitely many j ≥ 0 such that q j ∈ F .

3
Problem Formulation

Kinematic Model
Consider a UVMS which is composed of an AUV and a n DoF manipulator mounted on the base of the vehicle.The AUV can be considered as a six DoF rigid body with position and orientation vector where the components of the vectors have been named according to SNAME [24] as surge, sway, heave, roll, pitch and yaw respectively.The joint angular position state vector of the manipulator is defined by q := [q 1 , . . ., qn] ∈ R n .Define by q := [ q1 , . . ., qn] ∈ R n the corresponding joint velocities.
In order to describe the motion of the combined system, the earth-fixed inertial frame Σ I , the body-fixed frame Σ B and the endeffector fixed frame Σ E are introduced (see Figure 1).Moreover, without loss of generality, the reference frame Σ 0 is chosen to be located at the manipulator's base, and the frames Σ 1 , . . ., Σn are located to the 1-st, . . ., n-th link of the manipulator, respectively, under the Denavit-Hartenberg convention [25].The translational and rotational kinematic equations for the AUV system (see [1]) are given by: where: denote the position vector and the orientation vector of the frame Σ B relative to the frame Σ I , respectively; ν 1 , ν 2 ∈ R 3 denote the linear and the angular velocity of the frame Σ B with respect to Σ I , respectively; J(η 2 ) ∈ R 6×6 stands for the Jacobian matrix transforming the velocities from Σ B to Σ I ; J 1 (η 2 ), J 2 (η 2 ) ∈ R 3×3 are the corresponding parts of the Jacobian related to position and orientation, respectively; The notation sς and cς stand for the trigonometric functions sin(ς) and cos(ς) of an angle ς ∈ R, respectively.
Denote by q := η 1 , η 2 , q ∈ R 6+n the pose configuration vector of the UVMS.Let p, o ∈ R 3 be the position and orientation vectors of the end-effector with reference to the frame Σ I , respectively.The vectors p, o depend on the pose q and they can be obtained by the the following homogeneous transformation: where T j i is the homogeneous transformation matrix describing the position and orientation of frame Σ i with reference to the frame Σ j with i, j ∈ {1, . . ., n, I, 0, B, E}.The end-effector linear velocity ṗ ∈ R 3 and the time derivative or the Euler angles ȯ ∈ R 3 are related to the body-fixed velocities ν 1 , ν 2 and q with the following kinematics model: where In the latter, the stack vector ζ denotes the body-fixed system velocity vector.The Jacobian transformation matrices: are respectively defined by: In the latter, the vector pee ∈ R 3 is the local position of the endeffector with reference to the frame Σ B ; the matrices J e,1 , J e,2 ∈ R 3×n represent the manipulator Jacobian matrices with respect to the frame Σ 0 ; and S(•) the skew-symmetric matrix as given in Section 2.

Dynamic Model
The uncertain nonlinear dynamics of the UVMS are given by [1]: where: where: • M (q) ∈ R (6+n)×(6+n) is the inertia matrix for which it holds that: 6+n) is the matrix of Coriolis and centripetal terms; 6+n) is the matrix of dissipative effects; • d(q, ζ, t) ∈ R 6+n is a vector that models the external disturbances, uncertainties and unmodeled dynamics of the system; • g(q) ∈ R (6+n) is the vector of gravity and buoyancy effects; • u ∈ R 6+n denotes the vector of the propulsion forces and moments acting on the vehicle in the frame Σ B as well as the joint torques.
In order for the kinematic model ( 2) to be well-posed, state constraints are imposed such that: J + (q) := J(q)J(q) , J > 0, J > 0, J > 0, We further consider that the UVMS is in the presence of velocity and control input constraints captured by the sets: respectively.According to (1), the constraints q ∈ Q impose also constraints on the vector χ ∈ X ⊆ R 6 , where the set X can be computed by the transformation T(q), as given in (1).Note also that the function f given in ( 4) is assumed to be continuously differentiable in the set X × Z. Furthermore, we consider bounded disturbances d ∈ D where: For the kinematics/dynamics (2),(3), define the corresponding nominal kinematics / dynamics by: where and consider the linear nominal system: with: which is the outcome of the Jacobian linearization of the nominal dynamics (7a),(7b) around the equilibrium point η = 0. Due to the dimension of the control input (6 + n > 6), the linear system ( 8) is stabilizable.
Assume that the UVMS is operating in a bounded workspace W ⊆ R 3 in which there exist m ∈ N RoI labeled by M := {1, . . ., m}.Without loss of generality, the RoI are modeled by balls, i.e., Rm := B(ym, γm), m ∈ M where ym ∈ W and γm > 0 stand for the center and the radius of the m-th RoI, respectively.Define also the union of all RoI by: R := m∈M Rm. ( Due to the fact that we are interested in imposing safety specifications, at each time t ≥ 0, the UVMS occupies a ball B(η 1 (t), β) that covers its volume with radius β > 0. Assume that: which means that the RoI have sufficiently larger volume than the volume of the UVMS.

Objectives
The control objective is to control the UVMS with kinematics/dynamics as in ( 2), (3) such that it navigates between RoI of the workspace so that it obeys a high-level specification over atomic tasks.Atomic tasks are captured through a given set of atomic propositions Π.Each RoI is labeled with atomic propositions that hold true there.Define the labeling function: with R as given in (9), which maps each RoI with atomic propositions that hold true there.Note that some of the RoI can be assigned with labels that indicate unsafe regions, i.e., the UVMS is required to avoid visiting them (safety specifications).
Definition 6.Given a trajectory p(t) of the UVMS's end-effector, its corresponding behavior that is given by an infinite sequence of the form: is well-defined, if the following conditions hold: A trajectory of the end-effector of the UVMS p(t) satisfies an LTL formula ϕ over a set of atomic propositions Π formally, written as: if and only if there exists a well-defined behavior b = (p(t 0 ), 0 )(p(t 1 ), 1 )(p(t 2 ), 2 ) . . ., according to Definition 6, for which it holds that: according to LTL semantics given in Section 2.

Problem Statement
The problem treated in this paper can be now formalized as follows: Problem 1.Consider a UVMS composed of an AUV and an attached manipulator with n DoF, which is operating in a bounded workspace W ⊆ R 3 , governed by kinematics and dynamics given in (2) and (3), respectively.The workspace W contains the RoI Rm, modeled by the balls B(ym, γm), m ∈ M. Assume a high-level task ϕ expressed in LTL over the set of atomic propositions Π and labeling function L as in (10).The system is in the presence of state and input constraints as well as bounded disturbances which are respectively given by: Then, design a feedback control law u := κ(χ, ζ) such that the endeffector trajectory in the workspace p(t) fulfills the LTL specification ϕ, i.e., p(t) |= ϕ, ∀t ≥ 0, according to (11) of Definition 7.Moreover, the UVMS is required to remain in the workspace for all times.

Main Results
In this section, we propose a novel feedback control law that solves Problem 1 in a systematic way as follows: 1. Due to the fact that it is required to design a feedback control law that guarantees the transition of the UVMS between RoI under state and input constraints given by ( 12), as well as safety specifications, we utilize a Nonlinear Model Predictive Control (NMPC) framework [14][15][16].Furthermore, since the UVMS is under the presence of disturbances/uncertainties d ∈ D, we provide a novel robust analysis, the so-called tube-based NMPC approach.In particular, first, the error states and the corresponding transformed constraints sets are defined in Section 4.1.Then, the proposed feedback control law consists of two parts: an on-line control law which is the outcome of a solution to a Finite Horizon Optimal Control Problem (FHOCP) for the nominal system dynamics (see Section 4.3); and a state feedback law which is designed off-line and guarantees that the real system trajectories always lie within a hyper-tube centered along the nominal trajectories (see Section 4.2).
2. The dynamics (2),( 3) are abstracted into a TS, exploiting the fact that the runs of the TS project into well-defined behaviors according to Definition 6 (Section 4.4).3.By invoking ideas from correct by construction formal methods based control synthesis [10,13], a procedure that gives a sequence of control laws that serve as solution to Problem 1 is provided (Section 4.5).

Errors and Constraints
Consider that the UVMS with kinematics/dynamics as in ( 2),( 3) is occupying a RoI Rs ∈ R. The feedback control law needs to guarantee that the UVMS navigates towards a desired RoI Rd ∈ R, Rs = Rd without intersecting with any other RoI of the workspace, due to the fact that safety specifications are required.Define the vector χd := [p d , 0, 0, 0] ∈ R 6 , where p d ∈ R 3 stands for the center of the desired RoI Rd.Define the error state e := χ − χd ∈ R 6 .Then, the uncertain error kinematics/dynamics are given by: and the corresponding nominal error kinematics/dynamics by: By recalling that B(p(t), β) stands for the ball covering the UVMS at time t, define the set that captures the state-transition constraints by: The two constraints refer to the fact that the UVMS needs to remain in the workspace for all times and the fact that it should not intersect with any other RoI except Rs and Rd.In order to translate the constraints for the state χ ∈ X to constraints that are dictated regarding the error e, the constraints set is introduced, with ⊕ as given in Section 2.

Feedback Control Design
Consider the feedback law: the deviation between the real states of the uncertain system (13a),(13b) and the states of the nominal system (14a),(14b), respectively, with e(0) = z(0) = 0.It will be proved hereafter that the trajectories e(t), z(t) remain invariant in certain compact sets.The dynamics of the states e, z are written as: Since the aforementioned functions are continuously differentiable, the following hold: The constant Lc stands for the Lipschitz constant of function c with respect to the variable χ; L 1 , L 2 stand for the Lipschitz constants of function h with respect to the variables χ and ζ, respectively, and where k, σ > 0 are chosen such that the following hold: renders the sets: RCI sets for the error dynamics (16a), (16b), according to Definition 2, where the constants α 1 , α 2 > 0 are defined by: Proof: A backstepping control methodology will be used [26].The state z in (16a) can be seen as virtual input to be designed such that the Lyapunov function for the system (16a) is always decreasing.The time derivative of L 1 along the trajectories of system (16a) is given by: Design the virtual control input as z ≡ −σJ(q) e, with J, σ as given in ( 5), (18a), respectively.Then, by employing (5), (21) becomes: L(e) ≤ −σe J + (q)e + Lc e 2 2 ≤ −σλ min Define the backstepping auxiliary error state x := z + σJ(q) e ∈ R 6+n , and the the stack vector Consider the Lyapunov function Its time derivative along the trajectories of the system (16a),(16b) is given by: L(y) = e ė + x ż + σJ(q) ė + σ J(q) e = [e + σJ(q)x] ė + x ż + σx J(q) e = −σe J + (q)e + e b(•) + σx J(q) b(•) + e J(q)x + σx J + (q)x + σx J(q) e + x l(•) By invoking (22) as well as the following: with Λ 1 , Λ 2 given in (18b).By using Lemma 1 for n = P = 1, we get: with ρ designed so that (18a) holds.Combining the latter with ( 24) it yields: By designing u − u = −kx = −ke − kσJ(q) z, which is compatible with (15) and the same as in ( 17), we have: as α 1 and α 2 given in (20).Thus, it holds that: Taking the latter into consideration and the fact that y(0), we have that: , ∀t ≥ 0.Moreover, the following inequalities hold: , ∀t ≥ 0, which concludes the proof.
Remark 1.According to Lemma 2, the volume of the tube which is centered along the nominal trajectories e(t), ζ(t), that are solution of system (14a),(14b), depends on the parameters d, J, J, J, L and Lc.By tuning the parameters ρ and k from (18a) appropriately, the volume of the tube can be adjusted.Figure 2 shows a graphical representation of the tube-based approach.By using (15), the closed-loop system is written as: Assumption 1.It holds that: , i.e., there is sufficient space between any pairs of RoIs such that the UVMS can be navigated without intersecting them.
The aforementioned assumption is required in order for the navigation problem to have a solution.

On-line Optimal Control
Consider a sequence of sampling times {t k }, k ∈ N, with a constant sampling period 0 < h < T , where T is a prediction horizon.For the sampling times it holds that t k+1 := t k + h, ∀k ∈ N. At each sampling time t k , a FHOCP is solved for the nominal system (14a)-(14b) as follows: where: and Q, P ∈ R (12+n)×(12+n) and R ∈ R (6+n)×(6+n) are positive definite gain matrices.We will explain hereafter the sets E, V, U and F.
In order to guarantee that while the FHOCP (26a)-( 26d) is solved for the nominal dynamics (14a)-(14b), the real states e, ζ and control input u satisfy the corresponding state E, Z and input constraints U, respectively, the following modification is performed: with: 12+n) , the operators , • as defined in Section 2, and Ω 1 , Ω 2 as given in (19a), (19b), respectively.Intuitively, the sets E, Z and U are tightened accordingly, in order to guarantee that while the nominal states e, ζ and the nominal control input u are calculated, the corresponding real states e, ζ and real control input u satisfy the state and input constraints E, Z and U, respectively.This constitutes a standard constraints set modification technique adopted in tube-based NMPC frameworks (for more details see [19]).Define the terminal set by: which is used to enforce the stability of the system [27].In particular, due to the fact that the linearized nominal dynamics ( 8) are stabilizable, it can be proven that (see [27]) there exists a local controller which guarantees that the terminal set F is invariant.
Theorem 1. Suppose that Assumption 1 holds.Let the UVMS with dynamics as in (2), (3) occupy RoI Rs and p d be the center of a desired RoI Rd.Suppose also that the FHOCP (26a)-(26d) is feasible at time t = 0.Then, the feedback control law (15) renders the closed-loop system (25a)-(25b) Input to State Stable (ISS) with respect to the disturbance d(q, ζ, t).
Proof: The proof of the theorem consists of two parts:

Feasibility Analysis
It will be shown that recursive feasibility is established, and it implies subsequent feasibility.Before proceeding to the proof, consider the following definition.
if the following hold: Consider a sampling instant t k for which a solution u •; ξ(t k ) to the DFHOCP (26a)-(26d) of agent i ∈ V exists.Suppose now a time instant t k+1 such that t k+1 = t k + h, and consider that the optimal control signal calculated at t k is comprised of the following two portions: Both portions are admissible since the calculated optimal control input is admissible, and hence they both conform to the input Algorithm 1 Implementation of feedback control law u(t) Step 0: At time t 0 := 0, set ξ(0) = ξ(0) where ξ(0) is the current state.
Step 3: Measure the state ξ(t k+1 ) at the next time instant t k+1 of the system (13a), (13b) and compute the successor state ξ(t k+1 ) of the nominal system (14a), (14b) under the nominal control action u(t k ).
Step 4: constraints.Furthermore, the predicted states ξ(s; u (•), ξ(t k )) satisfy the state constraints for every s ∈ [t k , t k + T ] and it also holds that: Then, we can construct an admissible input u(•) starting at time t k+1 by sewing together the second portion of ( 27) and the input u i,loc (ξ) as: Applied at time t k+1 , u(s) is an admissible control input with respect to the input constraints as a composition of admissible control inputs, for all s ∈ [t k+1 , k+1 + T ].What remains to prove is the following statement.
Initially, at time t k+1 , u is an admissible control input according to Definition 8.Then, according to 3) of Definition 8 we have that: By invoking the fact that Ω 1 and Ω 2 are RCI sets it is guaranteed that: By combining ( 28) and ( 29), it yields that: By using Definition the properties of Section 2, we have that: from which it holds that: which concludes the proof of the statement.
By taking the aforementioned into consideration, the feasibility of a solution to the optimization problem at time t k implies feasibility at all times t n+1 , with n > k.Thus, since at time t = 0 a solution is assumed to be feasible, a solution to the optimal control problem is feasible for all t ∈ R ≥0 , and for all agents i ∈ V.

Convergence Analysis
Recall that: Then, we get: which, by using the fact that: as well as the bounds from (19a), (19b) the latter inequalities become: , ∀t ≥ 0. (30b) Since only the nominal system dynamics (14a)-(14b) are used for the online computation of the control action u(s) ∈ U, s ∈ [t k , t k + T ] through the FHOCP (26a)-(26d), by invoking nominal NMPC stability results found on [15,28], it can be proven that the NMPC control law u renders the closed loop trajectories of the nominal system (14a)-(14b) asymptotically ultimated bounded in the sets F.Then, from [23, Lemma 4.5, p. 150], there exists a class KL function β, such that: By combining (30a)-(30b) with (31) we get: , for every t ∈ R ≥0 .The latter inequalities leads to the conclusion of the proof.Remark 2. A major advantage of tube-based approach compared to other robust NMPC approaches, is that the FHOCP is solved only for the nominal system dynamics, thus the complexity is the same with the complexity of solving a nominal sampled-data NMPC (see [19] for more details) Algorithm 1 depicts the procedure of how the proposed control law is calculated and applied to the system.Theorem 1 implies that the UVMS with kinematics/dynamics as in ( 2), ( 3), starting at a RoI Rs, is driven by the controller (15) towards a desired RoI Rd, while all constraints imposed to the system are satisfied, i.e., the UVMS does not intersect with other RoI and always remains in the workspace W.

Discrete System Abstraction
The abstraction that captures the dynamics of the robot into a TS is given through he following definition: Definition 9.The motion of the UVMS in the workspace W is modeled by the TS T = (S, S 0 , Π, −→, Σ, L) where: • S = R = m∈M Rm is the set of states of the UVMS that contains all the RoI of the workspace W; • S 0 ∈ S is a set of initial states defined by the UVMS' s initial position in the workspace; The states are essentially the RoI or the workspace and the transitions represent the connected RoI.By construction, each behavior b := (p(t 0 ), 0 )(p(t 1 ), 1 )(p(t 2 ), 2 ) . . ., produced by the TS T , it is associated with the trajectory p(t) of the system (2)-( 3) and it is well-defined, as given in Definition 6.Hence, according to Definition 7, if we find a run of T satisfying the given LTL formula ϕ, we also find a desired behavior of the original system, and hence a trajectory p(t) that is a solution to Problem 1.
Remark 3. Note that, according to Assumption 1, the FHOCP (26a)-(26d) can always find a solution for the navigation between any two pairs of RoI, rendering thus the TS of Definition 9 a complete digraph.Therefore, there is no need to determine which pairs of RoI are connected by running a priori the FHOCP (26a)-(26d).The latter is used online for the actual navigation of the UVMS among the RoI output by the high-level planner, which is feasible due to Assumption 1.

Control Synthesis
There are several well-established methods to find an infinite run of T such that Trace(r) |= ϕ.In this paper, we adopt the method from [10], which is based on checking the emptiness of the product BA which will be defined hereafter.Definition 10.Let T be the TS from Definition 9 and Aϕ a BA that accepts the runs that satisfy the given formula ϕ.Then, the product automaton Ap := T ⊗ Aϕ is a tuple (Q , Q 0 , Act, δ , F ) where: We say that ((s, q), •, (s , q )) ∈ δ if (s, •, s ) ∈−→ and q ∈ δ(q, L(s)); • F := {(s, q f ) : s ∈ S and q f ∈ F }; Figure 3 depicts a framework under which a sequence of feedback control laws u(χ, ζ) that guarantee the satisfaction of the LTL formula ϕ can be computed.First, a BA Aϕ that accepts a run satisfying the specification formula ϕ is constructed.The related software tool can be found in [29].Second, the product automaton Ap := T ⊗ Aϕ according to Definition 10 is constructed.
The product automaton Ap is a graph.By performing graph search to the product automaton Ap, a run that satisfies the LTL formula ϕ and maps into a sequence of feedback control laws u(χ, ζ) of the form (15) can be found.
In the real UVMS system, when the robot is operating in a real environment, only the feedback control laws which are the outcome of the solution of the corresponding FHOCP (26a)-(26d) that guarantees the transitions from the previous step are calculated online.More specifically, the FHOCP (26a)-(26b) is the only computation that is performed online, at each sampling time of the system.Proposition 1.The solution that it is obtained from the aforementioned controller synthesis procedure provides a sequence of feedback control laws u(χ, v) as in (15) that guarantees the satisfaction of the formula ϕ of a UVMS governed by kinematics/dynamics as in (2)-( 3), thus, providing a solution to Problem 1.
According to (1), the transformation matrices which lead to the forward kinematics are given by: are given by the Denavit-Hantenberg parameters which can be calculated from Table 1.By imposing the constraints  The optimization horizon and the sampling time are set to T = 0.7 sec and h = 0.1 sec, respectively.The NMPC gains are set to Q = P = 0.5I 6 , R = 0.5I 10 .
In the workspace we have m = 14 RoI with radius γm = 0.6, ∀m ∈ M from which 6 of them stand for unsafe regions that the robot is prohibited to visit (depicted with red color in Figure 5).The RoI are located in the following positions on the workspace W: The unsafe regions are placed in specific positions such that the UVMS needs to maneuver to avoid them.The desired LTL formula is set to:

Conclusion
This paper addresses the problem of control of a UVMS under high-level tasks given in LTL.First, a tube-based NMPC framework that guarantees the robust transition of the UVMS between RoI Fig. 4: The GIRONA-UVMS composed of Girona500 AUV and ARM 5E Micro manipulator [7].
x [m]    of workspace under state, input and safety constraints, is provided.Then, by utilizing ideas from previous works, an abstraction and controller synthesis framework that gives a sequence of controllers that provably satisfy the given task is presented.Simulation results verify the efficiency of the proposed approach.Future research will be devoted towards extending the current framework into underwater object transportation of multiple UVMSs under event-triggered control communication.

Fig. 3 :
Fig. 3: A graphical illustration of the combined abstraction and controller synthesis framework.
over the set of atomic propositions Π = {obs, goal 1 , goal 2 }, and labeling function:L(R 5 ) = {goal 1 }, L(R 9 ) = {goal 2 }, L(R i ) = {obs}, i ∈ {2, 4, 6, 7, 8, 10}, L(R j ) = ∅, j ∈ {1, 3, 11, 12, 13, 14}.The formula intuitively means "Eventually reach goal 1 and then directly move to goal 2, while always avoid any obstacles of the environment".Figures5, 6depicts the workspace with RoI, unsafe regions, the nominal trajectory of the UVMS (orange color) and the real trajectory of the UVMS (black dashed color).According to Figures 5, 6 the robot never visits the unsafe RoI, eventually visits RoI R 5 and next visits R 9 .Thus, p(t) |= ϕ, ∀t ≥ 0, according to (11) of Definition 7. The error signals for the transition between RoI R 1 -R 3 are depicted in Figure7.The control effort for the transition from R 1 to R 3 is presented in Figure8.It can be observed that the desired task is performed while all the state/input constraints are satisfied.

Fig. 5 :
Fig. 5:The evolution of the trajectory of the UVMS in the workspace W. The RoI and unsafe regions are depicted with blue and red color, respectively.The real and the nominal trajectories χ(t) and χ(t), respectively, are depicted with orange and dashed black color.The UVMS successfully satisfies the task ϕ given in (32).

Fig. 6 :
Fig.6: A planar view of the workspace indicating that the UVMS is not intersecting with any unsafe RoI.

Table 1
Denavit-Hantenberg Parameters of the ARM 5E Micro