A Hybrid Compositional Approach to Optimal Mission Planning for Multi-rotor UAVs using Metric Temporal Logic
AA Hybrid Compositional Approach to Optimal Mission Planning forMulti-rotor UAVs using Metric Temporal Logic
Usman A. Fiaz and John S. Baras
Abstract — This paper investigates a hybrid compositional ap-proach to optimal mission planning for multi-rotor UnmannedAerial Vehicles (UAVs). We consider a time critical searchand rescue scenario with two quadrotors in a constrainedenvironment. Metric Temporal Logic (MTL) is used to formallydescribe the task specifications. In order to capture the variousmodes of UAV operation, we utilize a hybrid model for the sys-tem with linearized dynamics around different operating points.We divide the mission into several sub-tasks by exploiting theinvariant nature of various task specifications i.e., the mutualindependence of safety and timing constraints along the way,and the different modes (i,e., dynamics) of the robot. For eachsub-task, we translate the MTL formulae into linear constraints,and solve the associated optimal control problem for desiredpath using a Mixed Integer Linear Program (MILP) solver. Thecomplete path is constructed by the composition of individualoptimal sub-paths. We show that the resulting trajectorysatisfies the task specifications, and the proposed approachleads to significant reduction in computational complexity ofthe problem, making it possible to implement in real-time.
I. I
NTRODUCTION
Multi-rotor Unmanned Aerial Vehicles (UAVs) in generaland quadrotors in particular, find enormous applications inseveral key areas of research in academia as well as industry.These include but are not limited to search and rescue,disaster relief, surveillance, autonomous aerial transport,and entertainment. The motivation behind this expandingzeal towards multi-rotor UAVs is twofold. First, these arehighly inexpensive robots as compared to their fixed-wingcounterparts, and are therefore leading the way as a standardtestbed in much of the ongoing research in aerial robotics andmotion planning. Second, these are extremely agile robotscapable of much higher maneuverability in comparison withthe other UAV classes, namely fixed-wing and helicopterstyle UAVs. This salient feature edges them as a feasibleplatform to operate in congested environments as well, suchas crowded city skies and constrained indoor workspace.The enormous impact of quadrotors in autonomous searchand rescue missions becomes evident during natural disas-ters, where it is impossible for humans and terrestrial robotsto access highly cluttered spaces to rescue people from lifethreatening situations. In such scenarios, these agile multi-rotor UAVs come to our rescue. They can either locate andgrab a target themselves or can serve as a guide for otherrobots as well as humans to help evacuate a target. Forthis reason, an enormous amount of work has been donein designing new control strategies and planning algorithms
The authors are with the Department of Electrical and Computer Engi-neering and the Institute for Systems Research at University of Maryland,College Park, 20742 MD, USA. ( { fiaz,baras } @umd.edu ) for this specific class of aerial robots, with the aim to deploythem eventually in safety and time critical missions [1], [2].Given any high level task, it is a standard practice inclassical motion planning literature, to look for a trajectoryor a set of trajectories, which the robot can follow (i.e.,which its dynamics permit), while satisfying the desiredtask specifications [3]. This gives rise to the notion ofoptimal path planning, which considers an optimal pathin the sense of optimizating some suitable cost functionand a control law, to go from one position to another inspace while satisfying some constraints [4]. Traditionally,several interesting methods such as potential functions [5],and optimization through generation of probabilistic mapsin high dimensional state-space [6], [7], have been used forrobot mission planning. However, all these methods tend tofail in situations where the task involves some finite timeconstraints or dynamic specifications. Aerial surveying ofareas and time-critical search and rescue scenarios are twocommon examples of such tasks [8].Temporal logic [9]–[11] seems to address this problem,since it enables us to specify complex dynamic tasks incompact mathematical form. A bulk of modern motionplanning literature is based on Linear Temporal Logic (LTL)[12], [13], which is useful for specifying tasks such as visit-ing certain objectives periodically, surveying areas, ensuringstability and safety etc. [14]–[16]. However, from a controltheory perspective, LTL only accounts for timing in theinfnite horizon sense i.e., it can only guarantee somethingwill eventually happen and is not rich enough to describefinite time constraints. In addition, the traditional LTL formu-lation such as in [17], assumes a static environment, whichdoes not admit incorporating dynamic task specifications.On the other hand, Metric Temporal Logic (MTL) [18],[19], can express finite time requirements between variousevents of the mission as well as on each event duration.This allows us to specify safety critical missions with dy-namic task specifications and finite time constraints. Anoptimization based method for LTL was proposed in [20],and [21], where they translate the LTL task specificationsto Mixed Integer Linear Programming (MILP) constraints,which are then used to solve an optimal control problem fora linear point-robot model. This work was extended in [22],where the authors used bounded time temporal constraintsusing extended LTL and a timed automata approach formotion planning with linear system models. However, thisapproach did not incorporate a well defined or rich dynamicalmodel of the robot, and also illustrated significant computa-tional complexity issues for the proposed method. Similarly, a r X i v : . [ c s . R O ] S e p ptimization based methods with MTL specifications werepresented in [23] for single and multiple robots respectively.However, in both cases, the computation of the resultingtrajectory is expensive ( ∼
500 sec), and hence cannot beperformed in real-time. Moreover, these works have put highconstraints on the robot maneuverability, by limiting thedynamics to a simple linear (point-robot) model, which iscontradictory to the primary reason for deploying quadrotorsin constrained dynamic environments.In this work, we propose a compositional optimizationbased approach for motion planning with two UAVs in atime critical search and rescue mission scenario. We useMTL specifications to formally describe the task, and unlikeall of the described previous works, we utilize a completehybrid dynamical model of the robot (i.e., the quadrotor), tocapture its various modes of operation. We divide the missioninto several sub-tasks based on the different dynamics ofthe robot, and the invariant nature of safety and timingconstraints along the way. For each sub-task, we translatethe MTL formulae into mixed integer linear constraints, andsolve the associated optimization problem for desired optimalpath using a MILP solver. The final path is generated bycomposing the individual optimal sub-paths. We show thatby breaking down the motion planning problem into severalsub-problems, we can compute the optimal sub-paths in realtime while satisfying the safety and timing constraints. Thus,the main contribution of this paper is the demonstration of areal-time close-to-optimal mission planner for multiple UAVswith MTL specifications, while using a complete hybridmodel for the robot with minimum and realistic assumptions.The rest of the paper is organized as following. In Sec-tion II, we define the problem and describe the task usinga custom built workspace. Section III presents the notationused and some preliminaries on system dynamics, and MTL.In Section IV, we define the hybrid dynamical model for thequadrotor and discuss the linearization strategy for its variousmodes. Section V describes the formulation of the optimiza-tion problem for the task, and its decomposition into severalsub-tasks. Then, in Section VI, we detail our approach tosolving the problem. Section VII covers the results drawnfrom simulations. In Section VIII, we summarize and analyzethe outcomes of this work briefly, before concluding withsome future prospects.II. P
ROBLEM S TATEMENT
We consider a simple time critical search and rescuemission defined on a constrained workspace as shown inFig. 1. The workspace is custom designed in Autodesk In-ventor Professional environment, to closely suit the problemspecifications. The mission is desired as follows: Startingfrom initial positions A and B , two quadrotor UAVs q and q need to rescue (i.e., evacuate) two objects (shownin black and white circular disks) from a constrained envi-ronment respectively. The objects are located at F and G respectively, and are accessible only through a window E ,with dimensions such that it allows only one UAV to passat a given time. Quadrotor q has priority over quadrotor q Fig. 1. The computer aided design (CAD) model for the workspace used indefining the problem. The environment is a 10x10x3 cubic meter workspacewhich is divided into several 2D regions of interest that are labeled withalphabets, as well as marked with different colors. Red regions are reservedfor quadrotor q , while the blue regions are specific for quadrotor q . Theobjects to be evacuated are assigned in the same color code as well i.e.,black and white respectively for q and q . i.e., in the case that both quadrotors arrive at the window E at the same time, q has to wait for q to pass first. We alsoassume that there are no additional obstacles in the area otherthan the boundary walls O and the two UAVs. The specifictask for each UAV is to grasp its respective target object,and transport it to safety (marked H and H respectivelyfor q and q in the workspace) in given finite time. Whiledoing so, the UAVs need to avoid the wall O as well as eachother, in particular at the window E . In the later sections, wealso use prime region notation; for example, A (cid:48) to representthe same 2D region A . Here A (cid:48) represents an altitude (w.r.t z -axis) variation of the quadrotor while it is in the same 2Dregion A . III. N OTATION AND P RELIMINARIES
In this section, we describe some mathematical notationand preliminaries that are followed throughout, in rest of thepaper. These are essential to set up the described problemstatement as an optimal control problem with temporalconstraints.
A. System Dynamics
Any general (possibly nonlinear) dynamical system can berepresented in the form: ˙ x ( t ) = f ( t, x, u ) where for all time t continuous • x ( t ) ∈ X ⊆ R n is the state vector of the system • x (cid:44) x (0) ∈ X ⊆ X is the initial condition of thestate vector and • u ( t ) ∈ U ⊂ R m is the set of control inputs which isconstrained in the control set U .iven a nonlinear model of the system, a linearization aroundan operating point ( x ∗ ( t ) , u ∗ ( t )) is expressed as: ˙ˆ x ( t ) = A ( t )ˆ x ( t ) + B ( t )ˆ u ( t ) where for all time t continuous • ˆ x ( t ) = x ( t ) − x ∗ ( t ) • ˆ u ( t ) = u ( t ) − u ∗ ( t ) • A ( t ) = ∂f∂x ( t ) (cid:12)(cid:12)(cid:12) x = x ∗ ,u = u ∗ • B ( t ) = ∂f∂u ( t ) (cid:12)(cid:12)(cid:12) x = x ∗ ,u = u ∗ If time t is discretized, then the system dynamics take theform: x ( t + 1) = f ( t, x ( t ) , u ( t )) (1)where as before, x ( t ) ∈ X , x (0) ∈ X ⊆ X , and u ( t ) ∈ U for all t = 0 , , , · · · . Let us denote thetrajectory for System (1), with initial condition x at t ,and input u ( t ) as: x t ,x ,u ( t ) = { x ( t ) | t ≥ t , x ( t + 1) = f ( t, x ( t ) , u ( t )) , x ( t ) = x } . However, in this paper, for thesake of convenience, we use the shorthand notation i.e., x t instead of x t ,x ,u ( t ) to represent system trajectory wheneverno explicit information about u ( t ) and x is required. Similarto the the continuous time case, the corresponding linearizedsystem in discrete time looks like: ˆ x ( t + 1) = A ( t )ˆ x ( t ) + B ( t )ˆ u ( t ) (2)for all t = 0 , , , · · · . We use System (2) form dynamics inour final problem formulation in Section V. B. Metric Temporal Logic (MTL)
Now, we introduce some essential terminology and seman-tics for MTL. The convention on MTL syntax and semanticsfollowed in this paper is the same as presented in [18]. Moredetails on specifying tasks as MTL formulae can also befound in [19].
Definition 1:
An atomic proposition is a statement withregard to the state variables x that is either True ( (cid:62) ) or False ( ⊥ ) for some given values of x . [13]Let Π = { π , π , · · · π n } be the set of atomic propositionswhich labels X as a collection of areas of interest in theworkspace, which can be possibly time varying. Then, wecan define a map L which labels this (possibly) time varyingworkspace or environment as follows: L : X × I → Π where I = { [ t , t ] | t > t ≥ } and Π denotes the powerset of Π as usual. In general, I represents an interval oftime but it may just also represent a time instance. For eachtrajectory of System (1) i.e., x t as before, the correspondingsequence of atomic propositions, which x t satisfies is givenas: L ( x ) = L ( x (0) , L ( x (1) , ... .We later specify the tasks formally using MTL formulae,which can incorporate finite timing constraints. These formu-lae are built on the stated atomic propositions (Definition 1)by following some grammar. Definition 2:
The syntax of MTL formulas are defined inaccordance with the following rules of grammar: φ ::= (cid:62) | π | ¬ φ | φ ∨ φ | φ U I φ where I ⊆ [0 , ∞ ] , π ∈ Π , (cid:62) and ¬(cid:62) (= ⊥ ) are the Booleanconstants for true and f alse respectively. ∨ representsthe disjunction while ¬ represents the negation operator. U I denotes the Until operator over the time interval I .Similarly, other operators (both Boolean and temporal) canbe expressed using the grammar imposed in Definition 2.Some examples are conjunction ( ∧ ), always on I ( (cid:3) I ),eventually within I ( ♦ I ) etc. Further examples of temporaloperators can be found in [20]. Definition 3:
The semantics of any MTL formula φ isrecursively defined over a trajectory x t as: x t | = π iff π ∈ L ( x ( t ) , t ) x t | = ¬ π iff π / ∈ L ( x ( t ) , t ) x t | = φ ∨ φ iff x t | = φ or x t | = φ x t | = φ ∧ φ iff x t | = φ and x t | = φ x t | = (cid:13) φ iff x t +1 | = φx t | = φ U I φ iff ∃ t (cid:48) ∈ I s.t. x t + t (cid:48) | = φ and ∀ t (cid:48)(cid:48) ≤ t (cid:48) , x t + t (cid:48)(cid:48) | = φ .Thus, for instance, the expression φ U I φ means the follow-ing: φ is true within time interval I , and until φ is true, φ must be true. Similarly, the MTL operator (cid:13) φ meansthat φ is true at next time instance. (cid:3) I φ means that φ isalways true for the time duration or during the interval I , ♦ I φ implies that φ eventually becomes true within the interval I .More complicated formulas can be specified using a varietyof compositions of two or more MTL operators. For example, ♦ I (cid:3) I φ suffices to the following: within time interval I , φ will be eventually true and from that time instance, itwill always be true for an interval or duration of I . Theremaining Boolean operators such as implication ( ⇒ ) andequivalence ( ⇔ ) can also be represented using the grammarrules and semantics given in Definition 2 and Definition 3.Also, similar to the convention used in Definition 3, asystem trajectory x t satisfying an MTL specification φ isdenoted as x t | = φ . This is the general temporal constraintrepresentation, which we use later in the optimal controlproblem formulation.IV. Q UADROTOR D YNAMICS
We adopt the generalized nonlinear dynamics for thequadrotor model presented in [24]. We build a hybrid modelfor the system with five linear modes, namely
Take off , Land , Hover , Steer , and a task specific
Grasp mode. Thelinearization for each mode is carried out separately abouta different operating point. This enables our system to haverich dynamics with less maneuverability restrictions, whileeach mode still being linear. This is an important point andits significance becomes apparent once we formulate theproblem and present our solution approach, since it requiresall constraints in the problem to be linear. (see Section VI).
A. General Nonlinear Model
The dynamics of a quadrotor can be fully specified usingtwo coordinate frames. One is a fixed earth (or world) frame,nd the second is a moving body frame. Let the homoge-neous transformation matrix from body frame to earth framebe R ( t ) , which is a function of time t . In state space repre-sentation, the quadrotor dynamics are represented as twelvestates namely [ x, y, z, v x , v y , v z , φ, θ, ψ, ω φ , ω θ , ω ψ ] T , where ξ = [ x, y, z ] T and v = [ v x , v y , v z ] T represent the positionand velocity of the quadrotor respectively with respect tothe body frame. [ φ, θ, ψ ] T are the angles along the threeaxes (i.e., roll, pitch, and yaw respectively), and Ω =[ ω φ , ω θ , ω ψ ] T represents the vector containing their respec-tive angular velocities. Under the rigid body assumptions onits airframe, the Newton-Euler formalism for quadrotor inearth frame is given by: ˙ ξ = v ˙ v = − g e + Fm R e (3) ˙ R = R ˆΩ˙Ω = J − ( − Ω × J Ω + τ ) where J is the moment of inertia matrix for the quadrotor, g is the gravitational acceleration, e = [0 , , T , F is the totalthrust produced by the four rotors, and τ = [ τ x , τ y , τ z ] T isthe torque vector, whose components are the torques appliedabout the three axes. So, F , τ x , τ y , and τ z are the four controlinputs to System 3. B. Hybrid Model with Linear Modes
System (3) serves as a starting point for for generatinga hybrid model for the quadrotor with five modes, whichare represented by a labeled transition system as shown inFig. 2. As usual, the states (or modes) denote the actionof the robot, such as
Take off and
Steer , while the edgesrepresent the change or switch to another action. The changeis governed by some suitable guard condition. Note, thatsome edges donot exist; for example, the quadrotor cannotgo from
Land to Hover without taking the action
Take off .Each state or mode of the transition system follows certaindynamics, which result from a linearization of System (3)around a different operating point. As an example, considerthe
Hover mode. One possible choice of operating points forthe linearization of system dynamics in this mode is ψ = 0 .This implies that the two states ψ and ω ψ i.e., the yaw angleand its respective angular velocity are identically zero, andthus can be removed from the state space representation.Consequently, the state space dimension is reduced to ten,and the control set is reduced to three inputs as well; i.e., F, τ x , and τ y . The resulting linearized model can be writtenin standard (discrete time) form as: σ ( t + 1) = A ( t ) σ ( t ) + B ( t ) γ ( t ) , where σ ( t ) is the state, and γ ( t ) is the input (invector notation), with the two system matrices given as: A = I g − g
00 0
00 0 0 I ; B = /m
00 00 I × J − Fig. 2. The simplified hybrid dynamical model for the quadrotor. Someguard conditions are hidden for readability. We use linearized dynamicsaround different operating points for each mode. This makes the model richin dynamics as well as linear at the same time. where I , = [ I , , ] , and all zero and identity matricesin A ( t ) and B ( t ) are of proper dimensions. We adopt similarprocedure to linearize System (3) around other operatingpoints for different modes, and obtain linearized dynamicsfor the hybrid model. Here, we omit the discussion aboutthe selection of these operating points, but it can be foundin [24] and [25]. C. The Grasp Mode
Grasping in general is a very challenging problem, inparticular when dexterity based manipulation is involved.Since,
Grasp is the only task specific dynamical mode ofour system, it was advisable to simplify the grasping routinewithin the high level task. Thankfully, in case of aerialgrasping, some modern passive aerial grasping mechanisms[26] have been shown to be very reliable in grasping anobject with an instantaneous touchdown onto its surface[27]. Under this instantaneous touchdown and grasp assump-tion [28], we can express the
Grasp mode as a switchingcombination of
Hover , Land , and
Take off dynamics withspecial guard conditions. This clearly simplifies the problemof having the need to introduce a complex gripper and itsend-effector dynamics into the
Grasp mode. Figure 3 depictsthis representation of
Grasp mode in terms of the
Hover , Land , and
Take off dynamics.
Fig. 3. The
Grasp mode expressed as a combination of
Hover ( H ), land ( L ), and Take off ( T O ) modes (colored cyan), with special guardconditions. . P ROBLEM F ORMULATION
The described search and rescue problem can be set upas a standard optimal control formulation in discrete time.Given some general system dynamics (1), the objective is tofind a suitable control law that steers each quadrotor throughsome regions of interest in the workspace within desired timebounds, so that it evacuates the target safely. This controlalso optimizes some cost function, while the associatedtask constraints are specified by an MTL expression. Let φ denote the MTL formula for a task specification, and J ( x ( t, u ( t )) , u ( t )) be the cost function to be minimized. Forthe stated search and rescue scenario, a possible MTL taskspecification for quadrotor q can be: φ = ♦ [0 ,T ] F ∧ (cid:3) [0 ,T ] F (cid:48) ∧ ♦ [0 ,T ] (cid:3) H ∧ (cid:3) ¬ O ∧ (cid:3) ¬ q where T > T > T are discrete time units. A similarspecification can be written for quadrotor q . Then, thecorresponding optimization problem is given as: Problem 1: min x,u J ( x ( t, u ( t )) , u ( t )) subject to x ( t + 1) = f ( t, x ( t ) , u ( t )) x t | = φ Problem 1 is a discrete time optimal control problem withnon-linear system dynamics, which is difficult to solve evenin cases with simple φ . In our particular case, it is onlyharder. Thus, a standard practice is to use linear dynamicsat the expense of sacrificing maneuverability of the robot, inorder to simplify the problem (as is done in [29] etc.). Problem 2: min x,u J ( x ( t, u ( t )) , u ( t )) subject to x ( t + 1) = At ) x ( t ) + B ( t ) u ( t ) x t | = φ However, since we are using a hybrid model for the sys-tem, and not a linearization about a single operating pointonly, we are not sacrificing the dynamics and hence themaneuverability of the robot as much. Indeed we have fivedifferent modes or sets of linear dynamics associated withthe system. Therefore, now the question arises that given anMTL task specification φ , what linear dynamics or whichmode is best suited to solve Problem 2. The answer to thisquestion leads us to a very interesting conclusion. In general,the task specification φ is complex and asks the robot toperform actions which require the use of multiple modes.This is true in our problem as well. Therefore, an efficientway to solve Problem 2 is to break down the original taskinto several sub-tasks by exploiting their invariant propertiesalong the way, and based on the various modes associatedwith each sub-task.Thus, having a hybrid dynamical model with linear modes(which are linearizations of System (3) at different operatingpoints) enables us to break down the task specification φ into several sub-tasks. In addition, a closer inspection of φ reveals that all safety and timing constraints specified by φ need not be satisfied by either of the robots during the whole mission. There are some critical safety constraints such as”always avoid O ” that need to be satisfied most of the times,but majority of the constraints are purely local, based on theposition of the robot in the workspace. For example, a timingconstraint for quadrotor q to grasp the object at location F within 10 time units is independent from (or invariant of) atiming constraint for it to reach location C from A within 5time units, and so on. The mutual invariance of such localconstraints or sub-tasks can also be formally verified usingUPAAL [30] for instance.Thus, Problem 2 can be replaced by a collection ofsmaller optimization problems, each with a sub-task spec-ification represented as an MTL formula φ sub i , with theassociated ith -mode linear dynamics from the hybrid model.Further, we choose the cost function to be linear as welli.e., (cid:80) Nt =0 | u i ( t ) | , where N is the horizon for the optimaltrajectory. Thus, our final formulation of the problem is givenas: Problem 3: min x i ,u i (cid:80) Nt =0 | u i ( t ) | subject to x i ( t + 1) = A i ( t ) x i ( t ) + B i ( t ) u i ( t ) x t i | = φ sub i where φ sub i is the MTL specification for the ith sub-task, A i ( t ) , B i ( t ) are the linear system matrices for the ith mode,and x t i is the resulting optimal trajectory for the sub-task φ sub i . For example, for quadrotor q , one sub-task is to gofrom A to C in 5 time units. The MTL specification forthis sub-task is given by φ q AC ) = ♦ [0 , C ∧ (cid:3) ¬ O , and theassociated dynamics are selected from the Steer mode.VI. S
OLUTION A PPROACH
Having setup Problem 3, i.e., an optimization problemwith linear cost function and linear dynamics, we nowdescribe an approach to translate the MTL specification φ sub i to linear constraints. Our method is based on the approachpresented in [20] where the authors translate LTL specifi-cations to linear constraints. We will start with a simpletemporal specification and work through the procedure toconvert it to mixed integer linear constraints. We will thenuse this example as a foundation for translating other MTLoperators to equivalent linear constraints.Consider the constraint that a trajectory x ( t ) lies withina convex polytope K at time t . Since K is convex, it canbe represented as an intersection of a finite number ofhalfspaces. A halfspace can be represented as set of points, H i = { x : h Ti x ≤ a i } . Thus, x ( t ) ∈ K is equivalentto x ( t ) ∈ ∩ ni =1 H i = ∩ ni =1 { x : h Ti x ≤ a i } . So, theconstraint x ( t ) ∈ K ∀ t ∈ { t , t + 1 , · · · t + n } can berepresented by the set of linear constraints { h Ti x ( t ) ≤ a i } , ∀ i = { , , · · · , n } and ∀ t ∈ { t , t + 1 , · · · t + n } .In a polytopic environment, atomic propositions (see Defi-nition 2), p, q ∈ Π , are related to system state via conjunctionand disjunction of linear halfspaces [20]. Let us considerthe case of a convex polytope and let b ti ∈ { , } be somebinary variables associated with the corresponding halfspaces { x ( t ) : h Ti x ( t ) ≤ a i } at time t = 0 , ..., N . We can then forcehe constraint: b ti = 1 ⇐⇒ h Ti x ( t ) ≤ a i , by introducingthe following linear constraints: h Ti x ( t ) ≤ a i + M (1 − b ti ) (4) h Ti x ( t ) ≥ a i − M b ti + (cid:15) where M and (cid:15) are some large and small positive numbersrespectively. If we denote K K t = ∧ ni =1 b ti , then K K t = 1 ⇐⇒ x ( t ) ∈ K . This approach is extended to the generalnonconvex case by convex decomposition of the polytope.Then, the decomposed convex polytopes are related usingdisjunction operators. Similar to conjunction, as is describedlater in this section, the disjunction operator can also betranslated to mixed integer linear constraints.Let S φ ( x, b, u, t ) denote the set of all mixed integerlinear constraints corresponding to a temporal expression φ . Using the described procedure, once we have obtained S p ( x, b, u, t ) for atomic propositions p ∈ Π , we can for-mulate S φ ( x, b, u, t ) for any MTL formula φ . Now, forthe Boolean MTL operators, such as ¬ , ∧ , ∨ , let t ∈{ , , ..., N } , and as before K φt ∈ [0 , be the continuousvariables associated with the formula φ generated at time t with atomic propositions p ∈ Π . Then φ = ¬ p is the negationof an atomic proposition, and it can be modeled as: K φt = 1 − K pt (5)the conjunction operator, φ = ∧ mi =1 p i , is modeled as: K φt ≤ K p i t , i = 1 , ..., m (6) K φt ≥ − m + m (cid:88) i =1 K p i t and the disjunction operator, φ = ∨ mi =1 p i , is modeled as: K φt ≥ K p i t , i = 1 , ..., m (7) K φt ≤ m (cid:88) i =1 K p i t Similar to binary operators, temporal operators such as ♦ , (cid:3) ,and U can be modeled using linear constraints as well. Let t ∈ { , , ..., N − t } , where [ t , t ] is the time interval usedin the MTL specification φ . Then, eventually operator: φ = ♦ [ t ,t ] p is modeled as: K φt ≥ K pτ , τ ∈ { t + t , ..., t + t } (8) K φt ≤ t + t (cid:88) τ = t + t K pτ and always operator: φ = (cid:3) [ t ,t ] p is represented as: K φt ≤ K pτ , τ ∈ { t + t , ..., t + t } (9) K φt ≥ t + t (cid:88) τ = t + t K pτ − ( t − t ) and until operator: φ = p U [ t ,t ] q is equivalent to: c tj ≤ K jq j ∈ { t + t , · · · , t + t } c tj ≤ K lp l ∈ { t, · · · , j − } , j ∈ { t + t , · · · , t + t } c tj ≥ K jq + j − (cid:88) l = t K lp − ( j − t ) j ∈ { t + t , · · · , t + t } c tt = K tq (10) K φt ≤ t + t (cid:88) j = t + t c tj K φt ≥ c tj j ∈ { t + t , · · · , t + t } The equivalent linear constraints for until operator (10) areconstructed using a procedure similar to [20]. The modifica-tion for MTL comes from the following result in [18]. K φt = t + t (cid:95) j = t + t (cid:16) ( ∧ l = j − l = t K lp ) ∧ K jq (cid:17) . All other combinations of MTL operators for example,eventually-always operator: φ = ♦ [ t ,t ] (cid:3) [ t ,t ] p and always-eventually operator: φ = (cid:3) [ t ,t ] ♦ [ t ,t ] p etc., can be trans-lated to similar linear constraints using (5)-(10). In additionto the collective operator constraints, at the end, we need theconstraint K φ = 1 , which suffices to the overall satisfactionof a task specification φ . Further details on this method canbe found in [20].Using this approach, we translate the sub-task MTL con-straints i.e., φ sub i to a set of mixed integer linear constraints S φ ( x, b, u, t ) . This way Problem 3 converts into a MixedInteger Linear Program (MILP), which can be solved usinga solver, quite efficiently.Notice, that the worst case complexity of this MILP isexponential i.e., O (2 mN ) , where m is the number of booleanvariables or equivalently the number of halfspaces requiredto express the MTL formula, and N is the discrete timehorizon. This also backs our concept of decomposing thetask specification φ into several simpler sub-tasks φ sub i .VII. S IMULATION AND R ESULTS
We apply the described method for solving Problem 3collection, in the same workspace as shown in Fig. 1. Itis a custom built 10x10x3 cubic meter CAD environmentwhich is readily importable across many simulation andcomputation software tools. In our case study, the simulationexperiments are run through YALMIP-CPLEX using MAT-LAB interface on an Intel NuC. It is portable computer withan Intel core i7 @ 3.7 GHz CPU, an integrated Intel IrisGPU, and 16 GBs of memory.For the given problem statement (i.e., the task φ ), thesub-task specifications for the quadrotor q are specified asfollowing along with their respective dynamics. φ q AA (cid:48) ) = (cid:3) A ∧ ♦ [0 , A (cid:48) [ mode : Take off ] φ q AC ) = ♦ [0 , C ∧ (cid:3) ¬ O [ mode : Steer ] φ q CF ) = ♦ [0 , F ∧ (cid:3) ¬ O [ mode : Steer ] ig. 4. The resulting composed trajectories for the sub-tasks for q alone inthe workspace. Optimal trajectories for sub-tasks are marked with differentcolors. The Hover and
Steer altitude is set to . m which is the same asthe xz -center of the window E . A similar plot is obtained for q only withits respective regions of interest. φ q F F (cid:48) ) = (cid:3) F ∧ ♦ [0 , F (cid:48) [ mode : Grasp ] φ q F H ) = ♦ [0 , H ∧ (cid:3) ¬ O [ mode : Steer ] φ q H H (cid:48) ) = (cid:3) H [ mode : Land ] where O represents the obstacle. Using the convention de-fined earlier, the specification φ q AA (cid:48) ) requires the quadrotor q to attain desired threshold altitude (represented as A (cid:48) )while staying inside the 2D region marked A . φ q AC ) requires the quadrotor q to reach C within 5 time units,and φ q CF ) requires it to reach F within 10 time units,while avoiding the obstacle O . φ q F F (cid:48) ) requires the robot tograsp the object at F within 10 time units while staying in F ,whereas φ q F H ) asks the robot to reach H within 10 timeunits. Finally φ q H H (cid:48) ) forces q to stay at H indefinitely.Similarly, the sub-task specifications for quadrotor q aregiven as following. φ q BB (cid:48) ) = (cid:3) B ∧ ♦ [0 , B (cid:48) [ mode : Take off ] φ q BD ) = ♦ [0 , D ∧ (cid:3) ¬ O [ mode : Steer ] φ q D ) = (cid:3) D U ( ¬ ( pos ( q C ) [ mode : Hover ] φ q DG ) = ♦ [0 , G ∧ (cid:3) ¬ O [ mode : Steer ] φ q GG (cid:48) ) = (cid:3) G ∧ ♦ [0 , G (cid:48) [ mode : Grasp ] φ q GH ) = ♦ [0 , H ∧ (cid:3) ¬ O [ mode : Steer ] φ q H H (cid:48) ) = (cid:3) H [ mode : Land ] Notice, that quadrotor q has an additional constraint namely φ q D ) , which assigns priority to q by keeping q waiting at D , thus ensuring that both robots do not end up at E at thesame time. Fairness of this specification is easy to verify,since it is impossible for q to be stuck at C indefinitely.Furthermore, the Hover mode is implicitly present in all
Fig. 5. The resulting composed trajectories for the sub-tasks for q and q in the workspace simultaneously. Optimal trajectories for sub-tasks aremarked with different colors in reverse orders for q and q . The Hover and
Steer altitude is set to . m as before. The number of circular rings at D represent the number of time units spent by q while waiting in Hover mode. sub-tasks and is involved in switching between differentmodes. However, there is no corresponding optimizationrequired once the UAV is within the desired region. Forthe simulation, we use the cost function J as in Problem 3,with N = 30 i.e., the number of discrete times steps. Thequadrotor dynamics for all modes are uniformally discretizedat a rate of 5Hz.The resulting trajectory for the complete mission is gen-erated by concatenation of optimal sub-task trajectories.For quadrotor q alone, the final composed and continuoustrajectory (interpolated between discrete points) is shownin Fig. 4. The quadrotor successfully avoids the obstacleand evacuates the object to safety within specified time.In absence of q , a similar trajectory for quadrotor q isobtained with its respective areas of interest.Figure 5 shows the resulting composed trajectories forboth the quadrotors operating simultaneously. Again, bothsafely avoid the obstacle and evacuate their respective objectswithin given times. As expected, quadrotor q waits at D ,for quadrotor q , to avoid collision. The number of circularrings at D (see Fig. 5) correspond to the waiting time for q ,while it stays at D in Hover mode. Since, both these plotsare displaying motion in 3D, it is not possible to visualize thesatisfaction of timing constraints per each sub-task along afourth axis. However, we provide some insight to the timinganalysis of each sub-task for both quadrotors in terms ofcomputation time. In addition, a closer look at the simulationdata indicates that all timing constraints are indeed satisfied.Table I shows the computation times for each individualsub-task ( φ sub i ) for both quadrotors. These numbers indicatethat the proposed approach can be implemented in real-time.From an implementation point of view, the performance canbe further improved by using hardware which is optimizedfor computation (such as Nvidia Jetson TX2 etc.). ABLE IC
OMPUTATION TIMES FOR SUB - TASKS ( φ sub i ) Task ( q1 ) Time (sec)
Task ( q2 ) Time (sec) φ q AA (cid:48) ) φ q BB (cid:48) ) φ q AC ) φ q BD ) φ q CF ) φ q D ) φ q FF (cid:48) ) φ q DG ) φ q FH ) φ q GG (cid:48) ) φ q H H (cid:48) ) φ q GH ) φ q H H (cid:48) ) VIII. D
ISCUSSION
We have proposed a hybrid compositional approach tomission planning for quadrotors with MTL task specifica-tions, and have presented an optimization based methodwhich can be implemented in real time. Using a simple yetrealistic search and rescue test case, we have demonstratedthe computational efficiency of our approach, and haveshown that by breaking down the task into several sub-tasks,and using a hybrid model for system dynamics, it is possibleto solve the challenging problem of motion planning withrich system dynamics and finite time constraints in real time.In addition to some promising results, this work also posesmany new and interesting questions as well. For example,given a finite time constraint for the whole task, what is thebest or optimal way to divide the timing constraints amongvarious sub-tasks. Of course it is a scheduling problem, andis dependent on many factors such as robot dynamics, itsmaximum attainable speed, and nature of the sub-tasks aswell. In our study, the individual sub-task timing constraintassignments were relaxed and uniform. However, it is worthnoticing that using a complete dynamical model for the UAVputs less constraints on its maneuverability, and hence canallow it to tackle more conservative finite time constraintsas well. For example, the
Steer mode in our model allowsthe quadrotor to achieve speeds as high as 0.5 m/s, which isnot possible with the usual single mode
Hover linearizationonly, which involves small angle (pitch, roll) assumptions.The scalability features of this approach with regard toa large number of UAVs within a mission, need to bedemonstrated. The invariant property of sub-tasks can alsobe proved rigorously using tools from formal methods. Ex-tension of this work with more complicated tasks, dynamicobstacles, conservative constraints, and tolerances in bothtime and space can also yield interesting results, and areall great directions for future work.R
EFERENCES[1] J. Latombe,
Robot motion planning , ser. Kluwer international seriesin engineering and computer science. Boston: Kluwer AcademicPublishers, 1991.[2] S. M. LaValle,
Planning algorithms . Cambridge university press,2006.[3] U. A. Fiaz and J. S. Shamma, “usbot: A modular robotic testbed forprogrammable self-assembly,” arXiv preprint arXiv:1901.01497 , 2019.[4] H. M. Choset,
Principles of robot motion: theory, algorithms, andimplementation . MIT press, 2005.[5] W. Xi, X. Tan, and J. S. Baras, “A hybrid scheme for distributed controlof autonomous swarms,” in
Proceedings of the 2005 American ControlConference.
IEEE, 2005, pp. 3486–3491. [6] R. Sharma, “Locally efficient path planning in an uncertain, dynamicenvironment using a probabilistic model,”
Robotics and Automation,IEEE Transactions on , vol. 8, no. 1, pp. 105–110, 1992.[7] U. A. Fiaz and J. S. Shamma, “The usbot and programmable self-assembly,” in . IEEE, 2018.[8] A. Gholami, U. A. Fiaz, and J. S. Baras, “Drone-assisted communica-tions for remote areas and disaster relief,” in . IEEE, 2019.[9] C. Baier, J.-P. Katoen et al. , Principles of model checking . MIT pressCambridge, 2008, vol. 26202649.[10] E. M. Clarke, O. Grumberg, and D. Peled,
Model checking . MITpress, 1999.[11] M. M. Quottrup, T. Bak, and R. Zamanabadi, “Multi-robot planning:A timed automata approach,” in
Robotics and Automation, 2004.Proceedings. ICRA’04. 2004 IEEE International Conference on , vol. 5.IEEE, 2004, pp. 4417–4422.[12] C. Goerzen, Z. Kong, and B. Mettler, “A survey of motion planningalgorithms from the perspective of autonomous uav guidance,”
Journalof Intelligent and Robotic Systems , vol. 57, no. 1-4, p. 65, 2010.[13] R. Alur,
Principles of cyber-physical systems . MIT Press, 2015.[14] E. Frazzoli, M. A. Dahleh, and E. Feron, “Real-time motion planningfor agile autonomous vehicles,”
Journal of guidance, control, anddynamics , vol. 25, no. 1, pp. 116–129, 2002.[15] H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based reactive mission and motion planning,”
IEEE transactions onrobotics , vol. 25, no. 6, pp. 1370–1381, 2009.[16] E. Plaku and S. Karaman, “Motion planning with temporal-logicspecifications: Progress and challenges,”
AI communications , vol. 29,no. 1, pp. 151–162, 2016.[17] H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based reactive mission and motion planning,”
IEEE Transactions onRobotics , vol. 25, no. 6, pp. 1370–1381, 2009.[18] R. Koymans, “Specifying real-time properties with metric temporallogic,”
Real-time systems , vol. 2, no. 4, pp. 255–299, 1990.[19] J. Ouaknine and J. Worrell, “Some recent results in metric temporallogic,” in
Formal Modeling and Analysis of Timed Systems . Springer,2008, pp. 1–13.[20] S. Karaman, R. G. Sanfelice, and E. Frazzoli, “Optimal control ofmixed logical dynamical systems with linear temporal logic specifi-cations,” in . IEEE,2008, pp. 2117–2122.[21] E. M. Wolff, U. Topcu, and R. M. Murray, “Optimization-basedtrajectory generation with linear temporal logic specifications,” in
Int.Conf. on Robotics and Automation , 2014.[22] Y. Zhou, D. Maity, and J. S. Baras, “Timed automata approachfor motion planning using metric interval temporal logic,” in . IEEE, 2016, pp. 690–695.[23] A. Nikou, J. Tumova, and D. V. Dimarogonas, “Cooperative taskplanning of multi-agent systems under timed temporal specifications,”in . IEEE, 2016, pp. 7104–7109.[24] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The graspmultiple micro-uav testbed,”
IEEE Robotics & Automation Magazine ,vol. 17, no. 3, pp. 56–65, 2010.[25] L. R. Garc´ıa Carrillo, A. E. Dzul L´opez, R. Lozano, and C. P´egard,“Modeling the quad-rotor mini-rotorcraft,”
Quad Rotorcraft Control ,pp. 23–34, 2013.[26] U. A. Fiaz, N. Toumi, and J. S. Shamma, “Passive aerial graspingof ferrous objects,”
IFAC-PapersOnLine , vol. 50, no. 1, pp. 10 299–10 304, 2017.[27] U. A. Fiaz, M. Abdelkader, and J. S. Shamma, “An intelligent gripperdesign for autonomous aerial transport with passive magnetic graspingand dual-impulsive release,” in . IEEE, 2018,pp. 1027–1032.[28] U. A. Fiaz, “Passive magnetic latching mechanisms for robotic appli-cations,” Master’s thesis, KAUST, 2017.[29] Y. Zhou, D. Maity, and J. S. Baras, “Optimal mission planner withtimed temporal logic constraints,” in2015 European Control Confer-ence (ECC)