Co-design of Control and Planning for Multi-rotor UAVs with Signal Temporal Logic Specifications
CCo-design of Control and Planning for Multi-rotor UAVs with SignalTemporal Logic Specifications
Yash Vardhan Pant ∗ , He Yin ∗ , Murat Arcak, Sanjit A. Seshia Abstract — Urban Air Mobility (UAM), or the scenario wheremultiple manned and Unmanned Aerial Vehicles (UAVs) carryout various tasks over urban airspaces, is a transportationconcept of the future that is gaining prominence. UAM missionswith complex spatial, temporal and reactive requirements canbe succinctly represented using Signal Temporal Logic (STL),a behavioral specification language. However, planning andcontrol of systems with STL specifications is computationallyintensive, usually resulting in planning approaches that do notguarantee dynamical feasibility, or control approaches thatcannot handle complex STL specifications. Here, we presentan approach to co-design the planner and control such thata given STL specification (possibly over multiple UAVs) issatisfied with trajectories that are dynamically feasible and ourcontroller can track them with a bounded tracking-error thatthe planner accounts for. The tracking controller is formulatedfor the non-linear dynamics of the individual UAVs, and thetracking error bound is computed for this controller whenthe trajectories satisfy some kinematic constraints. We alsoaugment an existing multi-UAV STL-based trajectory generatorin order to generate trajectories that satisfy such constraints.We show that this co-design allows for trajectories that satisfya given STL specification, and are also dynamically feasiblein the sense that they can be tracked with bounded error.The applicability of this approach is demonstrated throughsimulations of multi-UAV missions.
I. I
NTRODUCTION
For Urban Air Mobility to become a reality in the nearfuture, the underlying planning and control approaches forthe Unmanned Aerial Vehicles (UAVs) carrying out varioustasks should be robust and have strong guarantees on safety.For this, the UAVs must respect spatial requirements, e.g.no-fly zones around geo-fenced areas and maintaining safedistances from each other, temporal requirements like visit-ing particular regions only during pre-defined time intervals,and reactive requirements, e.g. executing a contingency planin case of a system failure. It is hard to capture theserequirements with the objectives used in traditional controland planning approaches, however recent work on planningand control with Signal Temporal Logic (STL) [1]–[3] hasshown promise in representing these missions with STL,and also on developing algorithms for UAVs and fleets ofUAVs to satisfy these requirements. However in order to becomputationally tractable, these approaches either work withsimplifying abstractions of the system dynamics [1] or theworkspace [4], or cannot use the full expressiveness of STL Department of Electrical Engineering and Computer Sciences,University of California, Berkeley, USA [email protected],he [email protected], [email protected],[email protected] . ∗ The authors contributed equally.
TrackingController TrajectoryPlannerWorst casetracking error UAVTrajectories UAVKinematicsKinematicConstraintsState & InputConstraintsUAVDynamics STLSpecification
Fig. 1:
Overview of the co-design framework. The dashed linesrepresents information used in the offline design phase, while solidline represent information used during online execution. [3], e.g. use of the logical or operator to represent choicesbetween multiple tasks in a mission.In this paper, we present an approach that overcomesthese limitations by co-designing the planning and control formulti-rotor UAVs with missions represented as STL specifi-cations (see figure 1). To do so, we augment a state-of-the-arttrajectory planning approach [1] to generate trajectories suchthat a controller that considers the non-linear UAV dynamicscan track them. We also use sum-of-squares programming[5] to synthesize a controller that can track these trajectorieswith bounded error. We explicitly compute this bound anduse it in the trajectory planner to ensure that the plannedtrajectories robustly satisfy the STL specification, and thatthe system, when tracking with our controller, does notviolate the specification. A. Contributions of this work
The main contributions of our co-design approach are thatit ensures:1) The trajectories (generated via a centralized optimiza-tion) satisfy the STL specification in continuous time.2) They also respect pre-defined kinematic constraints inthe form of bounds on velocity, acceleration, and jerk.3) The non-linear dynamics and state/input constraintsof the UAVs are taken into account by the trackingcontroller, and worst-case tracking error bounds arecomputed.4) The trajectory planner satisfies the STL specificationwith a robustness margin, large enough to accommo-date the tracking error bound.Additionally, our approach can harness the full grammarof Signal Temporal Logic (see section II-B). Through twosimulation case studies, we show how this framework can beapplied to realistic scenarios and how the guarantees abovetranslate to actual control performance and robust satisfactionof the STL specification. a r X i v : . [ ee ss . S Y ] S e p . Related work Planning and Control with STL specifications.
Theproblem of controlling systems with STL specifications hasbeen studied extensively. [6], [7] build upon [8] to develop aMixed Integer Programming-based Model Predictive Controlapproach (MPC). These methods are developed with LinearTime Invariant (LTI) systems in mind, and are computation-ally complex to be applied for the control of multiple UAVswith complex STL specifications. [2] presents a non-convexoptimization-based MPC approach that is applicable to somenon-linear systems, but is also computationally heavy to beused for online control. A barrier function-based approachis presented in [3], but is applicable only to a fragmentof STL. Given the computational limitations in developingcontrollers for STL specifications, the work in [1] insteadaims to generate trajectories for multi-rotor UAVs such thatthey satisfy a given STL specification. It however offersonly kinematic feasibility of the planned trajectories, and notdynamic feasibility. Our co-design framework builds on thisapproach, and we show we can also ensure dynamic feasibil-ity through additional constraints, and through our design ofa tracking controller. There are also planning approaches that,unlike our approach, discretize the workspace and abstractaway the system dynamics (e.g. [4]).
Co-designing the planner and controller.
A hierarchicalcontrol framework has been explored by the path planningcommunity [9]–[11]. In this framework, a low-fidelity modelis used for planning online, and a high-fidelity model isused for designing a tracking controller offline, and thecontroller is then implemented online. A robust forwardreachable set used for planning is computed in [9] byconsidering the difference between the planning and trackingmodels. In [10], a Hamilton-Jacobi equation based method ispresented to synthesize a tracking error bound and an optimaltracking controller. An extension in [11] shows that for aclass of robotics systems, the problem can also be solvedusing sum-of-squares (SOS) programming. Related worksother than those from the path planing literature includethe continuous-state abstraction [12], [13], which providesa tracking controller and ensure the boundedness of theerror between the original high dimensional linear systemand its low dimensional abstraction. This idea is extendedto a general class of nonlinear systems in [14]–[16], and isapplied to UAVs in this paper.
C. Outline of the paper
In Section II we formalize the problem that we aim tosolve and provide a brief introduction to Signal Tempo-ral Logic and its associated robustness metric. Section IIIgives an overview of the co-design approach, introduces thesystem dynamics used for planning and control, and theassociated hierarchical control structure. Section IV presentsthe trajectory planner, which builds upon the work in [1],and discusses how the controller performance and kinematicconstraints are taken into account. Section V presents thesynthesis for the tracking controller that uses the full non-linear UAV dynamics, and its associated tracking error. Two case studies for multi-UAV settings with STL specificationsare presented in Section VI. Finally, we conclude with adiscussion of the limitations of this approach and future workin Section VII.
D. Notations
For ξ ∈ R n , R [ ξ ] represents the set of polynomials in ξ with real coefficients, and R m [ ξ ] and R m × p [ ξ ] to denote allvector and matrix valued polynomial functions. The subset Σ[ ξ ] := (cid:110) π = (cid:80) Mi =1 π i : M ≥ , π i ∈ R [ ξ ] (cid:111) of R [ ξ ] is theset of sum-of-squares (SOS) polynomials. The state of UAV i is denoted by x i ∈ R n . A trajectory of states over a timeinterval [0 , T ] is denoted by x i : [0 , T ] → R n . The value ofthe state at time t is denoted by x i ( t ) .II. P ROBLEM S TATEMENT AND PRELIMINARIES
A. Problem Statement
Formally, in order to achieve the objectives laid out earlierin the paper, we aim to solve the following problem:
Problem 1:
Given UAV dynamics ˙ x i ( t ) = f ( x i ( t )) + g ( x i ( t )) u i ( t ) and output y i ( t ) = C x i ( t ) for UAV i ∈{ , . . . , D } , control the UAVs such that resulting outputtrajectories ( y i ) are such that:1) ( y , . . . , y D ) | = ϕ ( y , . . . , y D ) , i.e. satisfy the STLspecification ϕ and,2) respect constraints x i ( t ) ∈ X ⊂ R n , u i ( t ) ∈ U ⊂ R m , ∀ i = 1 , . . . , D, ∀ t ∈ [0 , hrz ( ϕ )] .Here, x i ( t ) and u i ( t ) are the state and input for UAV i at time t , X and U are (hyper-rectangular) state andinput constraints, and hrz ( ϕ ) < ∞ is the time horizon ofthe specification ϕ , i.e. the minimum time duration neededto evaluate ϕ . For simplicity we assume that all UAVshave identical dynamics, however this not necessary for ourapproach to work.In order to avoid the pitfalls of existing approaches,we solve this problem by solving the two following sub-problems instead, which constitute a hierarchical planner-controller framework: Problem 2 (Trajectory planning):
Develop a planner thatuses the UAV kinematics of the form ˙ˆ x i ( t ) = A ˆx i ( t ) + B ˆu i ( t ) (details in Section III-A) to generate trajectories ˆx i such that:1) ( ˆx , . . . , ˆx D ) | = ϕ ( ˆx , . . . , ˆx D ) and,2) ˆx i ( t ) ∈ ˆ X, ˆu i ( t ) ∈ ˆ U ∀ i = 1 , . . . , D, ∀ t ∈ [0 , hrz ( ϕ )] ,3) any trajectories x i where || C ( x i ( t ) − G ˆx i ( t )) || ∞ ≤ δ, ∀ t ∈ [0 , hrz ( ϕ )] also satisfy ϕ .Here, G is a matrix of appropriate size that relates thestates in the kinematic model to those in the full dynamics,and δ ∈ R > is the worst-case tracking error bound.See Section V for details. The controller design is doneindividually for each UAV, so we drop the subscript i forease of notation: Problem 3 (Tracking Controller):
For the full UAV dy-namics (problem 1), develop a controller u ( t ) = k ( e ( t )) s.t.the error state e ( t ) = x ( t ) − G ˆx ( t ) for tracking a trajectory ˆx is s.t. || e ( t ) || ∞ ≤ δ , and x ( t ) ∈ X, u ( t ) ∈ U ∀ t ∈ [0 , hrz ( ϕ )] . . Review of Signal Temporal Logic (STL) Signal Temporal Logic (STL) [17] is a behavioral spec-ification language that can be used to encode requirementson signals. The grammar of STL [6] allows for capturing arich set of behavioral requirements using temporal operators,such as the
Until ( U ) operator and derived Always ( (cid:3) ) and Eventually ( ♦ ), as well as logical operators And ( ∧ ), Or ( ∨ ),and negation ( ¬ ). With these operators, an STL specification ϕ is defined over a signal, e.g. over the trajectories of theUAVs, and evaluates to either True or False . Example 1: (A two-UAV timed reach-avoid problem)
Twoquad-rotor UAVs are tasked with a mission with spatial andtemporal requirements in the workspace shown in Fig. 2:1) The two UAVs have to reach a Goal set (shown ingreen), or a region of interest, within a time of seconds after starting. UAV j (where j ∈ { , } ),with position (in 3D) denoted by p j , has to satisfy: ϕ reach ,j = ♦ [0 , ( p j ∈ Goal ) . The Eventually operatorover the time interval [0 , requires UAV j to be insidethe set Goal at some point within seconds.2) The two UAVs also have to avoid an Unsafe set (inred), or no-fly zone. For each UAV j , this is encodedwith Always and
Negation operators: ϕ avoid ,j = (cid:3) [0 , ¬ ( p j ∈ Unsafe )
3) Finally, the two UAVs should also be separated by atleast . meters along every axis of motion: ϕ separation = (cid:3) [0 , || p − p || ∞ ≥ . These requirements are combined into a two UAV timedreach-avoid specification: ϕ reach-avoid = ∧ j =1 ( ϕ reach ,j ∧ ϕ avoid ,j ) ∧ ϕ separation (1)In order to satisfy ϕ , a planning method generates tra-jectories ˆx , ˆx (where position p is obtained from the fullkinematic state ˆ x as p = Cx ) of a duration that is at leasthrz ( ϕ ) = 8 s, where hrz ( ϕ ) is the time horizon of ϕ . Ifthe trajectories satisfy the specification, i.e. ( p , p ) | = ϕ ,then the specification ϕ evaluates to True , otherwise it is
False . In general, an upper bound for the time horizoncan be computed as shown in [6]. Here, we only considerspecifications for which the horizon is bounded. More detailson STL are in [17] or [6].
C. Robustness of STL specifications
For every STL specification ϕ , we can construct a ro-bustness function [18] by following the grammar of STL.It outputs a robustness value ρ ϕ ( x ) for this formula, withrespect to the signal x that it is defined over, and has theimportant following property: Theorem 2.1: [18] For any x and STL formula ϕ , if ρ ϕ ( x ) < then x violates ϕ , and if ρ ϕ ( x ) > then x satisfies ϕ . The case ρ ϕ ( x ) = 0 is inconclusive.Thus, the degree of satisfaction or violation of a specificationis indicated by the robustness value. For simplicity, thedistances are defined in the inf-norm sense. This, combinedwith Theorem 2.1 gives us the following result: Corollary 1.1:
Given a trajectory ˆx such that ˆx | = ϕ withrobustness value ρ ϕ ( ˆx ) > , then any trajectory x that is y x -20 2 -44 -662 z
46 Unsafe Goal
Fig. 2:
The workspace, planned trajectories (red) and the trackedtrajectories (blue) for the two UAVs carrying out the timed reach-avoid mission of example 1. Simulation videos are at http://bit.ly/TimedRA . Color in digital version. within ρ ϕ ( ˆx ) of ˆx at each time, i.e. || ˆx ( t ) − x ( t ) || ∞ <ρ ϕ ( ˆx ) ∀ t ∈ [0 , hrz ( ϕ )] , is such that x | = ϕ (satisfies ϕ ). Significance:
We use this property in the co-design, wherethe planner generates a trajectory ˆx and the tracking con-troller tracks it with a trajectory x such that || C ( x ( t ) − G ˆx ( t )) || ∞ ≤ ρ ϕ (ˆ x ) ∀ t ∈ [0 , hrz ( ϕ )] . This requires the theinf-norm of the worst-case tracking error is always less than ρ ϕ ( ˆx ) . Note, ϕ is usually over a subset of states given by Cx ,e.g. the specification 1 is over the position components of theUAV state. In general, our approach allows for specificationsover the translational positions, velocities and accelerationsof the UAV (Section IV).III. O VERVIEW OF THE C O - DESIGN APPROACH
In order to solve problems 2 and 3, we adopt a hierarchicalplanning and control stack for the UAVs of the kind that iscommonly used, see figure 3. Our framework co-designs (seefigure 1) the two main components here, namely: • Trajectory planner that takes as input the STL specifi-cation defining the (possibly multi-UAV) mission. Usingthe UAV kinematics (Section III-A, the trajectory gen-erator solves an optimization (in a centralized manner)that generates trajectories for all UAVs involved. Thetrajectories satisfy the STL specification in continuoustime, ensure that kinematic constraints are respected,and take into account the worst-case tracking error.As long as the tracking controller does not deviatefrom a given bound, obtained from this optimization’sobjective, the tracking of these trajectories will besuch that the STL specification is satisfied. Section IVpresents the details. • Tracking controller that follows the trajectories gen-erated by the planner by taking into account both thenon-linear dynamics of multi-rotor UAVs (Section III-B) and the kinematics that the planner uses to generate rajectory Generator (Section 4)Tracking Controller(Section 5)Attitude and Motor Control STL Speci fi cation: ... Rotor speeds
Quad-rotor UAV 1
Tracking Controller(Section 5)Attitude and Motor Control
Quad-rotor UAV D
Rotor speeds
Fig. 3:
The planning and control hierarchy for multi UAV missions. trajectories. This controller is synthesized via a sum-of-squares optimization through which we obtain (state-dependent) tracking error bounds and worst-case track-ing error bounds, which are then used in the trajectoryplanning optimization. Details on the controller designare in Section V.Commands from the tracking controller are realized bylower level attitude controllers (Section III-D). The detailsof attitude controllers are beyond the scope of this paper,but standard approaches can be found in [19]. The resultingplanning and control hierarchy is shown in figure 3. Notethat the trajectory planning is done in a centralized manner,but the synthesized tracking controllers run independentlyon each UAV to track these generated trajectories. The restof this section introduces the UAV kinematics and dynam-ics used by the trajectory planner and tracking controller,respectively.
A. Multi-rotor UAV kinematics for trajectory planning
To generate UAV trajectories that satisfy a STL specifica-tion ϕ , we use the method in [1] that generates trajectories byconnecting segments of jerk-minimizing splines [20]. Theseare based on the following kinematic model. ˙ˆ x ( t ) = ˙ˆ p x ( t )˙ˆ p y ( t )˙ˆ p z ( t )˙ˆ v x ( t )˙ˆ v y ( t )˙ˆ v z ( t )˙ˆ a x ( t )˙ˆ a y ( t )˙ˆ a z ( t ) = ˆ v x ( t )ˆ v y ( t )ˆ v z ( t )ˆ a x ( t )ˆ a y ( t )ˆ a z ( t )000 + j x ( t )ˆ j y ( t )ˆ j z ( t ) (2) Here ˆ p x , ˆ v x , ˆ a x represents the position, velocity, andacceleration of the UAV in the global x coordinate (andsimilarly for y and z coordinates), and jerks ˆ j x , ˆ j y , and ˆ j z are the inputs ˆ u to this system. B. Quad-rotor UAV dynamics
For the UAVs, we consider the following non-linear dy-namics, adapted from [19, Chapter 2], ˙ x ( t ) = ˙ p x ( t )˙ p y ( t )˙ p z ( t )˙ v x ( t )˙ v y ( t )˙ v z ( t )˙ φ ( t )˙ θ ( t ) = v x ( t ) v y ( t ) v z ( t ) − f t ( t ) m c φ ( t ) s θ ( t ) f t ( t ) m s φ ( t ) g − f t ( t ) m c φ ( t ) c θ ( t ) u x ( t ) I x u y ( t ) I y , (3) where c φ := cos( φ ) , s φ := sin( φ ) (and similarly for θ ).The 8-state system has 6 translational states p x , p y and p z (the positions of the quad-rotor in a global co-ordinateframe) and v x , v y and v z (velocities), as well as two statesfor orientation φ and θ which are the roll and pitch anglesrespectively. To obtain this model, we assume that yaw ψ and its derivatives are zero throughout. The control inputs tothis model are f t (thrust), and u x and u y which are relatedto the torques generated by the rotors. We define a variable u to gather all the control inputs: u = [ f t , u x , u y ] (cid:62) . Withoutloss of generality, we assume the input constraint set is ofthe form: U = { u ∈ R : u ≤ u ≤ u } , where u, u ∈ R , and ≤ is applied elementwise. Here, m is the UAV’s mass, and I x and I y are the moments of inertia with respect to x and y axes in the body-fixed frame. Values for them are: m = 0 . ,and I x = I y = 0 . . C. Error dynamics for the tracking controller
To help keep track of the “distance” between the planningand tracking trajectories, we define the error state as: e = x − G ˆ x = p x − ˆ p x p y − ˆ p y p z − ˆ p z v x − ˆ v x v y − ˆ v y v z − ˆ v z φθ , (4) which include the positional and velocity differences, and theroll and pitch angles of the UAV. Here G = diag ( I , × ) .Later in Section V, we will design a tracking controller topenalize the error state e , and ensure its boundedness.The corresponding error dynamics are: ˙ e ( t ) = f e ( e ( t ) , ˆ x ( t )) + g e ( e ( t )) u ( t ) , (5)where f e ( e, ˆ x ) = e e e − ˆ a x − ˆ a y g − ˆ a z , g e ( e ) = − c e s e m s e m − c e c e m I x
00 0 I y . For simplicity, we assume all UAVs have identical dynamics, howeverour approach would apply otherwise as well. n the error dynamics, ˆ a x , ˆ a y and ˆ a z are treated as boundeddisturbances, whose bounds are enforced by the STL basedplanning algorithm described in Section IV. D. Lower level attitude and motor controllers
The attitude controller takes as measurements the roll andpitch rates ˙ φ, ˙ θ , and u x , u y as references. It is tasked withgenerating torques T x , T y and T z to realize u x and u y . ˙ u x ( t ) = T x ( t )˙ u y ( t ) = T y ( t ) T z ( t ) = ( I y − I x ) ˙ φ ( t ) ˙ θ ( t ) (6) Finally, from the computed values of f t , T x , T x and T z ,the desired rotor (angular) speeds ω i , i ∈ { , . . . , } can becomputed by solving the equality constrained system [19]: f t = b (cid:88) i =1 ω i T x = bl ( ω − ω ) T y = bl ( ω − ω ) T z = d ( ω + ω − ω − ω ) (7) Here, b , l , d are UAV specific constants [19]. The solutionto this system acts as the desired angular speeds for thelower-level motor controllers to track. The formulation ofthese lower-level controllers is beyond the scope of this work,and the interested reader can refer to [19] for details.IV. P LANNING FOR
UAV
S WITH
STL
SPECIFICATIONS
For trajectory planning with a given STL specification ϕ , defined possibly over trajectories of multiple UAVs,we use the method of [1] with some modifications. Thisapproach maximizes the robustness ρ ϕ (see Theorem 2.1) ofthe given STL specification by selecting position waypointsconnected by jerk minimizing splines [20]. The segmentbetween one waypoint, given by position ˆ p = [ˆ p x , ˆ p y , ˆ p z ] (cid:62) and velocity ˆ v = [ˆ v x , ˆ v y , ˆ v z ] (cid:62) , and another with desiredposition ˆ p = [ˆ p x , ˆ p y , ˆ p z ] (cid:62) , is a trajectory (see figure 4) offixed time duration T with the states of the kinematic model(2) ( ∀ l ∈ { x, y, z } , ∀ t ∈ [0 , T ] ) given by: ˆp l ( t ) ˆv l ( t ) ˆa l ( t ) = α t + β t + γ t + ˆ v l t + ˆ p lα t + β t + γ t + ˆ v lα t + β t + γt (8)Here, α , β and γ are linear functions of ˆ p , ˆ v and ˆ p (and parameter T ) [20]. We assume that the start and endaccelerations are zero, i.e. ˆ a = ˆ a = 0 × and end velocity ˆ v is not fixed. For brevity, we omit further details here. Theinterested reader can refer to [1], [20]. Trajectory planning optimization:
We use the Fly-by-Logic method [1] to generate a sequence of N + 1 po-sition waypoints in 3-D space for each UAV ( i ) ˆ p Ni =[ˆ p i , . . . , ˆ p Ni ] , where total flight time N T ≥ hrz ( ϕ ) , such thatthe resulting trajectories (see figure 4) of (8) ˆp i : [0 , N T ] → R maximize a continuously differentiable approximation( ˜ ρ ϕ ) [2] of the robustness ρ ϕ . Fig. 4: Jerk minimizing splines connecting position waypoints ˆ p , ˆ p and ˆ p . The arrows show the velocities at these waypoints ˆ v i , ∀ i ∈ { , , } . max ˆ p N ,..., ˆ p ND ˜ ρ ϕ ([ ˆp , . . . , ˆp D ]) (9a)s.t. ∀ i = 1 , . . . , D, ∀ j = 0 , . . . , N − (9b)LB vel (ˆ v ji ) ≤ ˆ p j +1 i − ˆ p ji ≤ UB vel (ˆ v ji ) (9c)LB acc (ˆ v ji ) ≤ ˆ p j +1 i − ˆ p ji ≤ UB acc (ˆ v ji ) (9d)LB jerk (ˆ v ji ) ≤ ˆ p j +1 i − ˆ p ji ≤ UB jerk (ˆ v ji ) (9e) ˜ ρ ϕ ([ ˆp , . . . , ˆp D ]) ≥ ˜ (cid:15) + δ (9f) This is a non-convex optimization, with linear constraints[1] and can be solved using off-the-shelf solvers like IPOPT.The UAV trajectories ˆp i , when discretized in time, arelinear functions of the position waypoints ˆ p Ni (detailsin [1]). Constraints (9c), (9d) are linear functions of thewaypoints (and associated waypoint velocities) to ensurethat the velocity and acceleration for each axis of motionare within predefined intervals [ v min , v max ] and [ a min , a max ] respectively. These constraints are from [1]. Additionallywe develop constraints (9e), also linear in the optimizationvariables, for the jerk to be within pre-defined intervals [ j min , j max ] as well. The expressions for these constraints arein the Appendix. Finally, the constraint (9f), where ˜ (cid:15) is theworst case approximation error [2] of ˜ ρ ϕ with respect to ρ ϕ ,is to ensure that the STL robustness of the trajectories withrespect to the specification ϕ is above a threshold δ > . Aswe will see in the next section, δ is the worst case trackingerror achieved by the tracking controller. Theorem 4.1 (STL satisfaction and kinematic feasibility):
A feasible solution to the optimization (9) generatestrajectories ˆp , . . . , ˆp D such that they:1) Satisfy the STL specification ϕ in continuous time2) Have bounded velocity, acceleration and jerk (alongevery axis of motion l ∈ { x, y, z } ) such that ∀ t ∈ [0 , hrz ( ϕ )] : ˆv l ( t ) ∈ [ v min , v max ] , ˆa l ( t ) ∈ [ a min , a max ] and ˆj l ( t ) ∈ [ j min , j max ] for each UAV.3) As long as each UAV tracks these trajectories withtracking error e ( t ) such that || e ( t ) || ∞ ≤ δ ∀ t ∈ [0 , hrz ( ϕ )] , the specification ϕ is satisfied.This trajectory planning approach hence serves as a solu-tion to problem 2. The proofs for the first two points (exceptthe bounded jerk constraints) can be found in [1]. The thirdpoint is a direct consequence of Corollary 1.1.V. C ONTROLLER AND E RROR B OUND S YNTHESIS
In (5), we have seen that the planner states enter the errordynamics. When designing a tracking controller, and comput-ing an error bound (to solve problem 3), the planner statesre treated as uncertain parameters. Additionally, from thesecond point of Theorem 4.1, we know that these uncertainparameters are bounded: ˆx ( t ) ∈ ˆ X ∀ t ∈ [0 , hrz ( ϕ )] , where ˆ X = R × [ v min , v max ] × [ a min , a max ] . With this informationin mind, the following proposition proposes a way of com-puting a tracking controller that ensures boundedness of thetracking error. Proposition 1:
Given error dynamics (5) with mappings f e : R n × R ˆ n → R n , g e : R n → R n × m , and η ∈ R , ˆ X ⊆ R ˆ n , u, u ∈ R m , if there exist a C function V : R n → R , and k : R n → R m , such that for all ˆ x ∈ ˆ X , the followingconstraints hold, V ( e ) = η ⇒ ∂V∂e · ( f e ( e, ˆ x ) + g e ( e ) k ( e )) ≤ , (10a) V ( e ) ≤ η ⇒ u ≤ k ( e ) ≤ u, (10b) then the sublevel set Ω Vη := { e ∈ R n : V ( e ) ≤ η } is forwardinvariant.The sublevel set Ω Vη is the error bound achieved bythe tracking controller k . In practice, it can be difficult tofind generic functions V and k satisfying constraints (10).Therefore, we use sum-of-squares (SOS) programming tosearch for them by restricting them to polynomials: V ∈ R [ e ] , and k ∈ R m [ e ] . This in turn requires the error dynamicsto be polynomials: f e ∈ R n [( e, ˆ a )] , g e ∈ R n × m [ e ] , where ˆ a := [ˆ a x , ˆ a y , ˆ a z ] (cid:62) . Additionally, define polynomials d l =( a max − ˆ a l )(ˆ a l + a min ) , l ∈ { x, y, z } .By applying the generalized S-procedure [5] to (10) to getits corresponding SOS constraints, and using the volume ofthe error bound as the cost function, we obtain the followingSOS optimization problem. min V,k,s i volume (Ω Vη ) s.t. s l ∈ Σ[( e, ˆ a )] , ∀ l ∈ { x, y, z } , s ∈ R [( e, ˆ a )] ,s j , s j ∈ Σ[ e ] , ∀ j ∈ { , ..., m } ,k ∈ R m [ e ] , V ∈ R [ e ] , (11a) − ∂V∂e · ( f e + g e k ) − (cid:88) l ∈{ x,y,z } s l d l − ( V − η ) s ∈ Σ[( e, ˆ a )] , (11b) u i − k i + ( V − η ) s j ∈ Σ[ e ] , ∀ j ∈ { , ..., m } , (11c) k i − u i + ( V − η ) s j ∈ Σ[ e ] , ∀ j ∈ { , ..., m } , (11d) where polynomials decision variables s i are called S-procedure certificates. The optimization (11) is non-convexas there are two groups of decision variables V and ( k, s , s j , s j ) bilinear in each other. We tackle it by usingthe so called alternating direction algorithm summarized in[14, Algorithm 1]. As a concrete example of the outputs ofoptimization (11), the tracking controller for u x (in (3)) is: u x ( e ) =0 . e e + 0 . e e + 0 . e e + 0 . e e − . e e + 0 . e e + 0 . e e + 0 . e e − . e e + 0 . e e − . e − . e − . e − . e − . e + 0 . e . Once the error bound Ω Vη is obtained, we can compute theworst-case tracking error δ by solving a convex optimization min δ> s.t. Ω Vη ⇒ − δ ≤ e i ≤ δ, i = 1 , ..., n. (12) Key result:
The worst-case tracking error δ acts as theinterface between the control and planning algorithms. Itsuse is formally stated below: Theorem 5.1:
Given kinematic constraint set ˆ X , resultingerror bound Ω Vη , tacking controller k , and worst-case trackingerror δ , if there exists a feasible planned trajectory satisfyingthe STL specification ϕ when solving the optimization (9)with ˆ X and δ , then the trajectories of the nonlinear UAVdynamics (3) under the control of k satisfy ϕ .This follows from the controller synthesis procedure, andthe trajectory planning guarantees of Theorem 4.1. The co-design framework ensures that planned trajectories satisfythe STL specification ϕ , and are dynamically feasible for thecontroller k to track without violating ϕ . Hence, together theplanner and controller serve as a solution to problem 1 . Co-design procedure:
We now summarize the co-designprocess, which is outlined in figure 1.1) Given the kinematic constraint set ˆ X and the UAVinput constraint set U , solve optimizations (11) and(12) to get k and δ (offline).2) Solve the optimization (9) to obtain planned trajecto-ries ˆx satisfying ϕ with robustness above δ (offline).3) Track ˆx for each UAV using k (online).VI. S IMULATIONS
We demonstrate our framework through two simulationexamples, adapted from [1]. The only other approach thatcan handle the full STL semantics, as well as non-lineardynamics is [2]. However these examples, especially the sec-ond example with 4 UAVs, are well beyond its computationalabilities. We do not perform comparisons to other approachesas they either are for planning only [1], [4], or cannot handlenon-linear dynamics [6], [7].
Simulation setup:
The trajectory planning algorithm and thecontroller synthesis was implemented in Matlab, and non-linear UAV dynamics (3) were used for the trajectory track-ing. The trajectory planning optimization is implementedusing Casadi, and solved using IPOPT as the solver. The tra-jectory computation times are of the order of a few secondsfor both examples. A thorough evaluation of computationtimes can be found in [1] . The SOS optimization (11)is formulated and translated into semi-definite program-ming (SDP) using SOSOPT, and the SDP is solved usingMOSEK. Variables V and k are parametrized using degree-2 polynomials in both examples, and the corresponding SOSoptimization takes . × seconds to solve (offline). A. Two-UAV Timed Reach Avoid
We start with the mission in Example 1, where two UAVsare tasked with a timed reach-avoid mission (see figure 2).
Results:
In the instance shown in figure 2, the UAVinitial states correspond to UAV 1 and 2 with positions [ − , , . (cid:62) and [ − , , (cid:62) respectively, with zero initial The control synthesis does not take state constraint X into accountdirectly, however it guarantees x ( t ) ∈ G ˆ X ⊕ B δ where B δ is a box withsides δ , G projects ˆ X on the state space x and ⊕ is the Minkowski sum. The additional constraints for bounded jerk add little to no overhead x ( m ) PlannedRobustness boundTracked y ( m ) Time (s) z ( m ) Fig. 5:
Position vs time for planned trajectories of UAV 2 flyingthe timed reach-avoid mission, and after tracking them via thesynthesized controller. The actual trajectories (tracked) are withinthe robustness bound of the planned trajectories, hence ensuringthat the specification is satisfied. velocities and orientations. The trajectory planner generatedtrajectories of robustness ρ ϕ reach-avoid ( ˆx ) = 1 . . The synthe-sized controller has a worst case tracking error (in the inf-norm sense) of δ = 1 . , implying the trajectories can betracked without violating (1). Ω Vη is shown with the blueellipsoids.Figure 5 shows the planned and tracked positions for UAV2, and figure 6 shows the tracking errors, demonstrating thatthe tracking is indeed good enough to ensure satisfaction ofthe underlying STL specification. This holds for both UAVs,for tracking of both positions and velocities, however wedo not present additional figures due to lack of space. Avideo playback of the simulation can be found at http://bit.ly/TimedRA B. Four-UAV Multi-mission example
Here, the four UAVs are tasked with performing two typesof mission in the workspace shown in figure 7. Two UAVs(1 and 2) have to perform a patrolling mission, twice visitingZone 1 and Zone 2 between pre-defined time intervals. Theother two UAVs (3 and 4) have to visit a Deliver regionwithin the first two seconds to drop off a package, and thenreach the Base region. For safety, all UAVs must avoid twoUnsafe regions and maintain pairwise separation of at least . m. This mission has a horizon of seconds, and isrepresented in STL as: ϕ multi-mission = ∧ i =1 ( ♦ [0 , ( p i ∈ Zone 1 ) ∧ ♦ [5 , ( p i ∈ Zone 2 ) ∧ ♦ [10 , ( p i ∈ Zone 1 ) ∧ ♦ [15 , ( p i ∈ Zone 2 )) ∧∧ ∧ i =3 ( ♦ [0 , ( p i ∈ Deliver ) ∧ ♦ [10 , ( p i ∈ Base )) ∧∧ ∧ i =1 ( (cid:3) [0 , ( ¬ p i ∈ Unsafe ) ∧ (cid:3) [0 , ( ¬ p i ∈ Unsafe )) ∧ i,j,i (cid:54) = j (cid:3) [0 , ( || p i − p j || ≥ . x Tracking error posTracking error velRobustness boundsWorst-case tracking error y Time (s) -202 z Fig. 6:
Tracking errors for position and velocity in the 3 axes ofmotion for UAV 2 (reach-avoid). Note that the errors are containedwithin the worst-case bounds, which in turn are smaller than thebounds imposed by the STL robustness of the trajectories.
Fig. 7:
Workspace for the four UAV multi-mission example. Sim-ulation videos are at http://bit.ly/MultiMission4UAV
Results:
The trajectory planner generates trajectories thatsatisfy the specifications with a robustness ρ ϕ multi-mission ( ˆx ) =1 . ≥ δ = 1 . , ensuring that the trajectories can betracked without violating the specification. Ω Vη is shownwith the blue ellipsoids in figure 7. The planned and trackedtrajectories are shown with red and blue curves, respectively.A video playback of the simulation can be found at http://bit.ly/MultiMission4UAV VII. C
ONCLUSION
Summary:
In this paper, we present an approach for plan-ning and control of (multiple) UAVs carrying out tasksspecified using Signal Temporal Logic (STL). This is doneby co-designing the trajectory generator and the controllerthat tracks these trajectories. We showed how this co-designallows us to take into account kinematic constraints, thefull non-linear UAV dynamics, and the tracking error whileenerating and flying out trajectories that satisfy the STLspecification in continuous time. Through simulations, weshow the the co-design results in trajectories that satisfy theSTL specification and a synthesized controller that tracksthem within prescribed bounds.
Limitations and future work:
In practice, the computedworst-case tracking bounds for the synthesized controller canbe very conservative. This is noticed in the simulations wherethe tracking error never approaches these bounds. This couldresult in the trajectory planning optimization being unableto obtain a high enough robustness value with respect to thegiven STL specification for Theorem 4.1 to hold, e.g. in caseswhere the UAVs need to fly through narrow openings, andrestricts the application of our approach to such settings. Toovercome this, we aim to improve our technique by tight-ening the STL specification by a state-dependent functionof the tracking error in the planning phase, instead of usingthe worst-case bounds on tracking error. Additionally, ourhierarchical planning and control framework is limited tospecifications only over the translational states of the systemand cannot handle temporal specifications on the orientationsof the UAVs. R
EFERENCES[1] Y. V. Pant, H. Abbas, R. A. Quaye, and R. Mangharam, “Fly-by-logic: control of multi-drone fleets with temporal logic objectives,” in
Proceedings of the 9th ACM/IEEE International Conference on Cyber-Physical Systems , pp. 186–197, IEEE Press, 2018.[2] Y. V. Pant, H. Abbas, and R. Mangharam, “Smooth operator: Controlusing the smooth robustness of temporal logic,” in
Control Technologyand Applications, 2017 IEEE Conf. on , pp. 1235–1240, IEEE, 2017.[3] L. Lindemann and D. V. Dimarogonas, “Control barrier functions forsignal temporal logic tasks,”
IEEE Control Systems Letters , 2019.[4] I. Saha, R. Rattanachai, V. Kumar, G. J. Pappas, and S. A. Seshia,“Automated composition of motion primitives for multi-robot systemsfrom safe ltl specifications,” in
IEEE/RSJ International Conference onIntelligent Robots and Systems , 2014.[5] P. Parrilo, “Structured semidefinite programs and semialgebraic geom-etry methods in robustness and optimization,” 2000.[6] V. Raman, A. Donze, M. Maasoumy, R. M. Murray, A. Sangiovanni-Vincentelli, and S. A. Seshia, “Model predictive control with signaltemporal logic specifications,” in , pp. 81–87, Dec 2014.[7] S. Sadraddini and C. Belta, “Robust temporal logic model predictivecontrol,” in
Allerton conference , September 2015.[8] A. Bemporad and M. Morari, “Control of systems integrating logic,dynamics, and constraints,”
Automatica , vol. 35, no. 3, pp. 407–427,1999.[9] S. Kousik, S. Vaskov, F. Bu, M. Johnson-Roberson, and R. Vasude-van, “Bridging the gap between safety and real-time performance inreceding-horizon trajectory design for mobile robots,” arXiv preprintarXiv:1809.06746 , 2018.[10] S. L. Herbert, M. Chen, S. Han, S. Bansal, J. F. Fisac, and C. J. Tomlin,“FaSTrack: A modular framework for fast and guaranteed safe motionplanning,” in , pp. 1517–1522, Dec. 2017.[11] S. Singh, M. Chen, S. L. Herbert, C. J. Tomlin, and M. Pavone,“Robust tracking with model mismatch for fast and safe planning: ansos optimization approach,” arXiv preprint arXiv:1808.00649 , 2018.[12] A. Girard and G. J. Pappas, “Hierarchical control system design usingapproximate simulation,”
Automatica , vol. 45, no. 2, pp. 566 – 571,2009.[13] S. W. Smith, M. Arcak, and M. Zamani, “Approximate abstractionsof control systems with an application to aggregation,”
Automatica ,vol. 119, p. 109065, 2020.[14] S. W. Smith, H. Yin, and M. Arcak, “Continuous abstraction ofnonlinear systems using sum-of-squares programming,” in , pp. 8093–8098,2019. [15] P.-J. Meyer, H. Yin, A. H. Brodtkorb, M. Arcak, and A. J. Sørensen,“Continuous and discrete abstractions for planning, applied to shipdocking,” arXiv e-prints , p. arXiv:1911.09773, Nov. 2019.[16] H. Yin, M. Bujarbaruah, M. Arcak, and A. Packard, “Optimizationbased plannertracker design for safety guarantees,” in , pp. 5194–5200, 2020.[17] O. Maler and D. Nickovic,
Monitoring Temporal Properties of Con-tinuous Signals . Springer Berlin Heidelberg, 2004.[18] G. Fainekos and G. Pappas, “Robustness of temporal logic specifica-tions for continuous-time signals,”
Theor. Computer Science , 2009.[19] F. Sabatino, “Quadrotor control: modeling, nonlinearcontrol design,and simulation,” 2015.[20] M. W. Mueller, M. Hehn, and R. D ´Andrea, “A computationallyefficient motion primitive for quadrocopter trajectory generation,” in
IEEE Transactions on Robotics , 2015. A PPENDIX
For the constraints of the trajectory planning optimization(9), consider the min-jerk trajectory segment [20], of timeduration T , between a waypoint ˆ p j = [ˆ p jx , ˆ p jy , ˆ p jz ] (cid:62) with ve-locity ˆ v j = [ˆ v jx , ˆ v jy , ˆ v jz ] (cid:62) , and the next waypoint with desiredposition ˆ p j +1 = [ˆ p x , ˆ p y , ˆ p z ] (cid:62) . We define the functions [1],for t ∈ [0 , T ] : K ( t ) = (90 t ) / (48 T ) − (90 t ) / (12 T ) + (30 t ) / (4 T ) K ( t ) = (90 t ) / (12 T f ) − (90 t ) / (4 T ) + (30 t ) / (2 T ) (13) Let t (cid:48) = argmax t ∈ [0 ,T ] K t ( t ) . We can now define the con-straints (for each UAV) that ensure velocity and accelerationare within bounds [ v min , v max ] and [ a min , a max ] respectively,for each axis of motion l : LB vel (ˆ v jl ) = ( v min − (1 − T K ( T ))ˆ v jl ) /K ( T ) UB vel (ˆ v jl ) = ( v max − (1 − T K ( T ))ˆ v jl ) /K ( T ) LB acc (ˆ v jl ) = T ˆ v jl + a min /K ( t (cid:48) ) UB acc (ˆ v jl ) = T ˆ v jl + a max /K ( t (cid:48) ) (14) Combining these constraints for all axis of motiongives x, y, z the velocity and acceleration constraintsin the optimization (9) of the form LB vel (ˆ v j ) =[ LB vel (ˆ v jx ) , LB vel (ˆ v jy ) , LB vel (ˆ v jz )] and similarly for the upperbound for velocities and upper/lower bounds for acceleration.These constraints are such that: LB vel (ˆ v j ) ≤ ˆ p j +1 − ˆ p j ≤ UB vel (ˆ v j ) ⇒ ˆ v jl ∈ [ v min , v max ] ∀ t ∈ [0 , T ] , ∀ l ∈ { x, y, z } , and,LB acc (ˆ v j ) ≤ ˆ p j +1 − ˆ p j ≤ UB acc (ˆ v j ) ⇒ ˆ a jl ∈ [ a min , a max ] ∀ t ∈ [0 , T ] , ∀ l ∈ { x, y, z } (15) Finally, the constraint that ensures bounded jerk ˆj l ( t ) ∈ [ j min , j max ] ∀ t ∈ [0 , T ] for each axis of motion l are: LB jerk (ˆ v jl ) = min((2 T / j min − . T ˆ v jl , (2 T / j min + T ˆ v jl ) UB jerk (ˆ v jl ) = min((2 T / j max − . T ˆ v jl , (2 T / j max + T ˆ v jl ))