Critical Missions of Multiple UAVs - ISR-Lisboa

Path Generation, Path Following and Coordinated Control for TimeCritical Missions of Multiple UAVs I. Kaminer,* O. Yakimenko,* A. Pascoal,** and R. Ghabcheloo** Abstract — The paper proposes a solution to the problem of coordinated control of multiple unmanned air vehicle (UAV) to ensure collision-free maneuvers under strict spatial and temporal constraints. First, a set of feasible trajectories are generated for all UAVs using a new direct method of optimal control that takes into account rules for collision avoidance. A by-product of this step yields, for each vehicle, a spatial path to be followed together with a nominal desired speed profile. Each vehicle is then made to execute a pure path following maneuver in three-dimensional space by resorting to a novel 3D algorithm. Finally, the speed profile for each vehicle is adjusted to enforce the temporal constraints that must be met in order to coordinate the fleet of vehicles. Simulations illustrate the potential of the methodology developed. *

Keywords: Unmanned Air Vehicles, Coordinated Motion Control, Trajectory Generation, Path Following, Graph Theory.

T

I.

INTRODUCTION

his paper addresses the problem of coordinated control of multiple unmanned air vehicles (UAVs) under tight spatial and temporal constraints. This topic of research is motivated by the need to develop strategies for coordinated ground target suppression and sequential auto-landing for multiple UAVs. Both mission scenarios require a group of UAVs to execute time-critical maneuvers in close proximity of each other. For the case of ground target suppression, a formation of UAVs must break up and execute a coordinated maneuver to arrive at a predefined position over the target at a given time. Similarly, for the case of sequential autolanding, a formation must also break up and arrive at the assigned glideslope point separated by pre-specified safeguarding time intervals. A key requirement underlying these missions is that all maneuvers be collision-free. In recent years, there has been widespread interest in the problem of coordinated motion control of fleets of autonomous vehicles. Applications include aircraft and spacecraft formation flying [1]-[4], coordinated control of land robots [5]-[6], and control of multiple surface and underwater vehicles [7]-[10]. The work reported in the literature addresses a large class of topics that include, among others, leader/follower formation flying, control of the center of mass and radius of dispersion of swarms of vehicles, and uniform coverage of an area by a group of surveying robots. There are however applications with UAVs that do not fit the scenarios commonly described in the literature. Namely, the missions described in the present work that include spatial as well as temporal requirements. *

Dept. of Mechanical and Astronautical Engineering, Naval Postgraduate School, Monterey, CA, USA. ** Institute for Systems and Robotics (ISR) and Dept. Electrical Engineering and Computers, Instituto Superior Técnico (IST), Lisbon, Portugal.

To deal with the new scenarios, a methodology for coordinated control of UAVs is proposed that unfolds in three basic steps. First, a set of feasible trajectories are generated for all UAVs using a direct method of optimal control that takes explicitly into account the boundary initial and final conditions, the simplified UAV dynamics, and safety rules for collision avoidance. This is done by resorting to an extension of the work reported in [11] to multiple UAVs. A by-product of this step yields - for each vehicle - a spatial path to be followed, together with a desired nominal speed profile along that path. The second step consists of making each vehicle execute a path following maneuver along its assigned path by resorting to a new nonlinear path following algorithm in three dimensional space that generalizes the one introduced in [5] for wheeled robots. Finally, the speed profile of each vehicle is adjusted to enforce the temporal constraints that must be met in order to coordinate the fleet of vehicles. Clearly, the methodology proposed relies on the decoupling of space and time in the problem formulation. The rationale for this procedure stems from the fact that path following controllers are easier to design than trajectory tracking controllers and, when properly designed, yield smooth approaching maneuvers to the spatial curves that must be tracked. At the same time, this strategy will naturally generate the control activity that is required for each vehicle to capture its nominal path generated during the path planning phase, even if due to unforeseen disturbances the vehicle deviates too much from it. The paper is organized as follows. Section II describes the methodology adopted for real-time, feasible UAV trajectory generation. Section III offers a solution to the problem of path following in 3D. Section IV addresses the coordination of a fleet of AUVs in time. Finally, Section V includes the results of simulations with the nonlinear dynamic models of a small fleet of UAVs. II. FEASIBLE TRAJECTORY GENERATION This section describes an algorithm for multiple UAV real-time trajectory generation that is based on the results reported in [11-14] for a single aircraft. The new algorithm developed allow for the computation of feasible trajectories for multiple UAVs that are de-conflicted in space and which can be tracked by resorting to the path following algorithm described in Section III. In what follows we denote by p c (τ ) := ⎡⎣ xc (τ ) , yc (τ ) , zc (τ ) ⎤⎦

T

a desired trajectory to be

followed by a single UAV, parameterized by some parameter, say the virtual arc τ ∈ ⎡⎣0;τ f ⎤⎦ , where τ f is the

total virtual arc length that for our purposes we view as an optimization parameter. We assume that the coordinates xc (τ ) , yc (τ ) , zc (τ ) can be represented by algebraic polynomials of degree N of the form N

xi (τ ) = ∑ aikτ k , i = 1, 2,3

(1)

k =0

where, for notational convenience, we set x1=x, x2=y, and x3=z. The degree N of the polynomials xi (τ ) is determined by the number of boundary conditions that must be satisfied. Let d 0 and d f be the highest-order of the derivatives of xi (τ ) that must meet specified boundary constraints at the

initial and final points of a trajectory, respectively. Then, the minimum degree N* of each polynomial in (1) is N ∗ = d 0 + d f + 1 . For example, if the desired trajectory includes constraints on initial and final positions, velocities and accelerations (second-order derivatives) then the degree of each polynomial is N*=2+2+1=5. Additional degrees of ∗ freedom may be included by making N > N . As an

illustrative example, Table 1 shows how to compute the polynomial coefficients in (1) for 5th and 6th order polynomial trajectories. The second column differs from the first one in that fictitious initial jerk constraints are included. This increases the order of the resulting polynomial but affords extra optimization (design) parameters xi′′′0 ; i = 1, 2,3 . Figure 1a shows examples of admissible 5th-order polynomial trajectories when the optimization parameter τ f

is varied (all boundary constraints are satisfied by default, but there is no control over the higher-order derivatives: N k! xi′′′0 := 6ai 3 , xif′′′ = ∑ aik τ kf −3 , i = 1, 2,3 ). Figure 1b k − ( 3)! k =3 shows how an increase in the number of optimization parameters leads to a larger class of admissible trajectories (in this particular case we acquire control over the initial jerk). It is important to point out that the parameterization (1) completely determines the UAV’s spatial profile, i.e. a 3D trajectory that satisfies all boundary conditions by construction.

Table 1. Examples of computation of the coefficients of polynomial trajectories. Boundary conditions d0/df N*/N

xi 0 , xi′0 , xi′′0 , xif , xif′ , xif′′ 3 (adding fictitious jerk xi′′′0 )/2 5/6

2/2 5/5

Linear algebraic matrix equation to solve for the coefficients aik

⎛1 0 ⎜ ⎜0 1 ⎜0 0 ⎜ ⎜1 τ f ⎜0 1 ⎜ ⎜ ⎝0 0

0

0

0

0

0

0

2

0

0

τ 2f 2τ f 2

τ 3f τ 4f 3τ 2f 4τ 3f 6τ f 12τ 2f

0 ⎞ ⎛ ai 0 ⎞ ⎛ xi 0 ⎞ ⎟ ⎜ ⎟ 0 ⎟ ⎜⎜ ai1 ⎟⎟ ⎜ xi′0 ⎟ 0 ⎟ ⎜ ai 2 ⎟ ⎜ xi′′0 ⎟ ⎟⎜ ⎟ = ⎜ ⎟ τ 5f ⎟ ⎜ ai 3 ⎟ ⎜ xif ⎟ 5τ 4f ⎟ ⎜ ai 4 ⎟ ⎜ xif′ ⎟ ⎟⎜ ⎟ ⎜ ⎟ 20τ 3f ⎠⎟ ⎝⎜ ai 5 ⎠⎟ ⎝⎜ xif′′ ⎠⎟

⎛1 0 ⎜ ⎜0 1 ⎜0 0 ⎜ ⎜0 0 ⎜1 τ f ⎜ ⎜0 1 ⎜ ⎝0 0

0 0 2 0

τ 2f 2τ f 2

0 0 0 6

0 0 0 0

τ 3f τ 4f 3τ 2f 4τ 3f 6τ f 12τ 2f

0 0 0 0

τ 5f 5τ 4f 20τ 3f

⎞ ⎛ ai 0 ⎞ ⎛ xi 0 ⎞ ⎟⎜ ⎟ ⎜ ′ ⎟ ⎟ ⎜ ai1 ⎟ ⎜ xi 0 ⎟ ⎟ ⎜ ai 2 ⎟ ⎜ xi′′0 ⎟ ⎟⎜ ⎟ ⎜ ⎟ ⎟ ⎜ ai 3 ⎟ = ⎜ xi′′′0 ⎟ 6 ⎟⎜ τf ai 4 ⎟ ⎜ xif ⎟ ⎟ ⎜ ⎟ ⎜ ⎟ 6τ 5f ⎟ ⎜ ai 5 ⎟ ⎜ xif′ ⎟ 30τ 4f ⎠⎟ ⎝⎜ ai 6 ⎠⎟ ⎝⎜ xif′′ ⎟⎠ 0 0 0 0

a) b) Figure 1. Admissible trajectories for 5th (a) and 6th (b) order polynomials with a single ( τ f ) and multiple ( τ f and xi′′′0 ,; i = 1, 2,3 ) optimization parameters, respectively.

The fact that the virtual arc was selected as a parameter in (1) allows for additional flexibility. If τ := t , then defining spatial profiles means defining speed profiles along a trajectory as well. This is due to the fact that vehicle speed is related to the time derivatives of the trajectory coordinates as v ( t ) = x&12 (t ) + x&22 (t ) + x&32 (t ) .

(2)

Figure 2 shows the speed profiles corresponding to trajectories in Fig.1 for the case of τ := t . This figure also

demonstrates that increasing the number of optimization parameters allows one to obtain speed profiles that do not exceed predefined constraints. A feasible trajectory is one that can be tracked by an UAV without having it exceed predefined bounds on the velocity v ( t ) along the corresponding path as well as on the total acceleration. Let vmin , vmax and amax denote predefined

bounds on the velocity and total acceleration, respectively. dτ . Then, from (2) we obtain Further define λ (τ ) = dt v (τ ) = λ (τ ) x1′2 (τ ) + x2′2 (τ ) + x3′2 (τ ) = λ (τ ) pc′ (τ ) . (3)

Notice that λ (τ ) can be chosen as a polynomial of a degree sufficiently high to satisfy boundary conditions on speed. Then, a trajectory pc (τ ) is feasible if

The optimization problems F1, F2 can be effectively solved in real-time by adding a penalty function G as discussed in [11] and by using any zero-order method. As an example, Fig.3 illustrates the flexibility afforded by the reference polynomials to compute a coordinated target suppression mission by three UAVs (moving from right to left). 4000 3500

vmin ≤ λ (τ ) pc′ (τ ) ≤ vmax , and

3000 UAV 3

UAV 2

2500

pc′′(τ )λ (τ ) + pc′ (τ )λ ′ (τ ) λ (τ ) ≤ amax , ∀τ ∈ ⎡⎣ 0,τ f ⎤⎦ . (4)

y(m)

2

2000 1500 UAV 1 1000 500 0 -3000

-2000

-1000

0

1000 x(m)

2000

3000

4000

5000

Fig.3. Coordinated attack trajectories.

III. PATH FOLLOWING OF POLYNOMIAL TRAJECTORIES a)

b) Figure 2. Speed profiles corresponding to trajectories shown on Fig.1 in case τ := t .

Let Ξ be the vector of optimization parameters: τ f , xi′′′0 , i = 1,...,3 and polynomial coefficients defining λ (τ ) . A

feasible trajectory can be obtained by solving, for example, the following optimization problem ⎪⎧min τ f F1: ⎨ Ξ ⎪⎩ subject to (4) For the case of multiple UAVs, say n, the dimension of the problem increases. To guarantee spatial deconfliction, one set of constraints may be added to account for a minimum separation distance E between each pair of vehicles. Let τ fi denote the total path length of the ith UAV and let wi > 0 . Feasible, spatially deconflicted trajectories for each vehicle can thus be obtained by solving the optimization problem ⎧ ⎪Ξmin ∑ wiτ fi ⎪ i ,i =1, n i =1, n F2: ⎪⎨ subject to (4) for each i ∈ [1, n ] and ⎪ 2 ⎪ min pcj (τ j ) − pck (τ k ) ≥ E 2 , ∀ (τ j ,τ k ) ∈ [0,τ fj ]× [0,τ fk ] j , k =1,..., n ⎪ j≠k ⎩

The algorithm for trajectory generation introduced in Section II yields - for each vehicle - a spatial path to be followed, together with the corresponding nominal speed profile. To follow the paths computed, this section describes a path following algorithm that extends that in [5] to a 3D setting and introduces a further modification aimed at meeting time-critical inter-vehicle constraints. At this level, only the simplified kinematic equations of the vehicle will be addressed by taking pitch rate and yaw rated as virtual outer-loop control inputs. The dynamics can be dealt with at a later stage by introducing an inner-loop control law. However, this will not be analyzed formally in the paper. The notation required is introduced next, with reference to Fig.4. Let {F} a Serret-Frenet frame attached to a generic point on the path and let {W} be the wind frame attached to the UAV (a frame that has its x-axis aligned with the UAV´s velocity vector). Further let F ωFI denote the angular velocity of {F} with respect to inertial frame {I}, resolved in {F}. Denote by pc (τ ), where τ denotes the arc length introduced in previous section, the path to be followed and by Q denote the center of mass of the aircraft. Let P be an arbitrary point on the path that plays the role of the center of mass of a “virtual” aircraft to be followed. This is in contrast with the set-up for path following originally proposed in [15] where P was simply defined as the point on the path that is closest to the vehicle. Since this point may not be uniquely defined, the strategy in [15] led to very conservative estimates for the region of attraction about the path to be followed. Endowing P with an extra degree of freedom (that will be exploited later) is the key to the algorithm presented in [5] that is extended in this paper to the 3D case. Notice that Q can be resolved in {I} as qF = [ s1

y1

qI = [ x

y

z]

T

or in {F} as

z1 ] . With the above notation, the simplified T

UAV kinematic equations can be written as

⎧ x& = v cos γ cosψ ⎪ y& = −v cos γ sin ψ ⎪ ⎪ & (5) ⎨ z = v sin γ ⎪ ⎡ γ& ⎤ ⎡1 0 ⎤ ⎡q ⎤ ⎪ ⎢ ⎥=⎢ ⎪⎩ ⎣ψ& ⎦ ⎣ 0 cos −1 γ ⎥⎦ ⎢⎣ r ⎥⎦ where v is the magnitude of the UAV’s velocity vector, γ is the flight path angle, ψ is the heading angle, and and r are the y-axis and z-axis components, respectively of the vehicle’s rotational velocity resolved in wind frame {W}. With a slight abuse of notation, q and r will be often referred to as pitch rate and yaw rate, respectively in the wind frame {W}. Vs Inertial Frame {I} zI Desired path to follow

UAV

Q zF(B)

yF(N) y1

qI yI

qF

z1

xF(T)

s1 P Serret-Frenet Frame {F}

pc xI

Fig.4. Problem geometry.

F I

⎡ x& ⎤ ⎡ τ&(1 − κ y1 ) + s&1 ⎤ R ⎢⎢ y& ⎥⎥ = ⎢⎢ y&1 + τ&(κ s1 − ς z1 ) ⎥⎥ ⎢⎣ z& ⎥⎦ ⎢⎣ ⎥⎦ &1 z&1 + ς sy

=> & & s − τ (1 − κ y1 ) ⎤ ⎡ 1⎤ ⎡ ⎡v ⎤ ⎢ y& ⎥ = ⎢ −τ&(κ s − ς z ) ⎥ + F R ⎢ 0 ⎥ , 1 1 ⎥ W ⎢ 1⎥ ⎢ ⎢ ⎥ ⎣⎢ z&1 ⎦⎥ ⎣⎢ −ςτ& y1 ⎦⎥ ⎣⎢ 0 ⎦⎥ where we used the fact that ⎡ x& ⎤ ⎡ x& ⎤ ⎢ ⎥ ⎢ &⎥ F F W & I R ⎢ y⎥ = W R I R ⎢ y⎥ = ⎢⎣ z& ⎥⎦ ⎢⎣ z& ⎥⎦

(8)

(9)

⎡v ⎤ (10) R ⎢⎢ 0 ⎥⎥ . ⎢⎣ 0 ⎥⎦ Let λe denote the Euler angles φe , θ e ,ψ e that parameterize locally the rotation matrix from {F} to {W}. Then, λ&e = Q −1 (λe )W ωWF , where ⎡ ⎢1 sin φ tan θ e e ⎢ Q −1 (λe ) = ⎢ 0 cos φe ⎢ sin φe ⎢0 ⎢⎣ cos θ e

F W

⎤ cos φe tan θ e ⎥ ⎥ − sin φe ⎥ ⎥ cos φe ⎥ cos θ e ⎥⎦

(11)

Remark 1. – In the kinematic model (5), q and r play the π W role of virtual control inputs. Clearly, the dynamics of the is nonsingular for θ e ≠ ± 2 and ωWF denotes the angular UAV are not taken into consideration at this level. This is in velocity of {W} with respect to {F} resolved in {W}. Note line with the control strategies adopted in a vast number of that W ωWF = W ωWI −W ωFI and W ωFI = WF R F ωFI . flight control systems with an inner-outer (dynamickinematic) loop configuration. In these cases, a controller is Therefore, λ&e = Q −1 (λe ) ( W ωWI − WF R(λe ) F ωFI ) (12) available to stabilize the inner loop and to track the angular rate commands issued by the outer loop. We therefore and concentrate on the design of the outer (kinematic) loop. sinψ eςτ& ⎡ θ&e ⎤ ⎡ ⎤ However, a complete inner-outer setup is used in Section V ⎢ ⎥=⎢ & ⎥+ − + τ ς tan θ cos ψ κ & ( ) ψ to drive a full 6DOF nonlinear model of an UAV. e e ⎦ ⎣ e⎦ ⎣ Following standard nomenclature [16]-[17], (13) ⎡ cos φe − sin φe ⎤ ⎡q ⎤ ⎡q ⎤ dp(τ ) dp(τ ) dT (τ ) dT (τ ) ⎢ ⎥ , N (τ ) := and T (τ ) := / / ⎢ sin φe cos φe ⎥ ⎢ r ⎥ =: D + G ⎢ r ⎥ . dτ dτ dτ dτ ⎣ ⎦ ⎢⎣ cos θ e cos θ e ⎥⎦ ⎣ ⎦ B (τ ) = T (τ ) × N (τ ) π denote the tangent, normal, and binormal, respectively to the where D and G are defined for all θ e ≠ ± . Let 2 path at the point determined by τ . The vectors T, N, B are u ⎧ ⎫⎪ q ⎡ ⎤ orthonormal and define the basis vectors of {F} as well as ⎡ ⎤ ⎪ θ = G −1 ⎨ ⎢ ⎥ − D ⎬ . (14) I ⎢ ⎥ the rotation matrix F R = [T N B ]. from {F} to {I}. It is well ⎣r ⎦ ⎪⎩ ⎣uψ ⎦ ⎪⎭ dT (τ ) T is Then, combining equations (9) and (14) yields the equations known that F ωFI = [ςτ& 0 κτ& ] , where κ (τ ) := for the (path following) error dynamics: dτ ⎧ s&1 = −τ&(1 − κ y1 ) + v cosθ e cosψ e dB (τ ) is its torsion. the curvature of Pc (τ ) and ς (τ ) := ⎪ dτ ⎪ y&1 = −τ&(κ s1 − ς z1 ) + v cosθ e sinψ e ⎪ With the notation above (see also Fig.4) (15) Ge = ⎨ z&1 = −ςτ& y1 − v sin θ e I qI = pc (τ ) + F RqF , (6) ⎪& ⎪θ e = uθ and therefore ⎪ψ& e = uψ ⎩ ⎛ ⎡τ& ⎤ ⎡ s&1 ⎤ ⎡ s&1 ⎤ ⎞ ⎟ Notice how the rate of progression dτ/dt of point P along ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ I I I ⎜ F (7) q& I = F R ⎢0 ⎥ + F R ⎢ y&1 ⎥ + F R ⎜ ωFI × ⎢ y&1 ⎥ ⎟ the path becomes an extra variable that can be manipulated ⎜ ⎢⎣0 ⎥⎦ ⎢⎣ z&1 ⎥⎦ ⎢⎣ z&1 ⎥⎦ ⎟⎠ at will. With the above notation, the path following problem ⎝ can now be formulated in a rigorous manner. The objectives => are to drive the center of the mass of the UAV to P and to

τ& = K1 s1 + v cos θ e cosψ e

align its total velocity with the tangent to the path at that point. A similar formulation was used in [18] to address path following for mobile robots. Problem 1 (Path Following) Given a spatially defined polynomial path pc (τ ) (see Section II) and a velocity

uθ = − K 2 (θ e − δθ ) +

uψ = − K 3 (ψ e − δψ ) −

profile v(t ) > 0 , ∀t ≥ 0 , determine feedback control laws

⎛ z1 ⎞ ⎛ ⎞ Define δθ = sin −1 ⎜ θ a and δψ = sin −1 ⎜ψ a y1 ⎟ ⎜ z + d ⎟⎟ ⎜ y1 + d 2 ⎟⎠ 1 1 ⎠ ⎝ ⎝ for some 1 ≥ θ a > 0, 1 ≥ ψ a > 0, d1 > 0 and d 2 > 0. Then the feedback control laws

should be viewed as “desired approach angles” aimed at shaping the transient phase of the approach to the path. Their choice is inspired by previous work of Samson et al. [15] on path following for wheeled robots. Computing the timederivative of V yields

ψ e − δψ s s& y y& z z& θ − δ ψ& e − δ&ψ V& = 1 1 + 1 1 + 1 1 + e θ θ&e − δ&θ + c1 c1 c1 c2 c3 =

sinψ e − sin δψ & c3 y1v cos θ e + δψ , ψ e − δψ c1

for some c1 > 0 , , c2 > 0 and , c3 > 0 Notice how the Lyapunov function captures the deviations of the translational errors from zero and those of the rotational errors from the desired values of δθ and δψ . The latter

Suppose the speed v(t) of the UAV satisfies v(t ) > vmin , ∀t ≥ 0 .

)

(16)

with K1 > 0, K 2 > 0, K 3 > 0 drive s1 , y1 and z1 , together with θ e , ψ e asymptotically to zero. Proof. Consider a candidate Lyapunov function 2 1 2 1 1 2 V= ( s1 + y12 + z12 ) + (θe − δθ ) + (ψ e − δψ ) , 2c1 2c2 2c3

for q, r and τ& to drive the linear errors s1 , y1 and z1 and the rotational errors θ e , ψ e asymptotically to zero. A solution to the above path following problem is presented below. Proposition 1. Consider the kinematic equations of an UAV given in (5) and let pc (τ ) be a path to be followed.

(

sin θ e − sin δθ & c2 z1v + δθ c1 θ e − δθ

(

)

s1 y z ( −τ&(1 − κ y1 ) + v cos θ e cosψ e ) + 1 ( −τ&(κ s1 − ς z1 ) + v cos θ e sinψ e ) + 1 ( −ςτ& y1 − v sin θ e ) c1 c1 c1

+

θ e − δθ

+

θ e − δθ

c2 c2

(

(17)

ψ e − δψ s y z uθ − δ&θ + uψ − δ&ψ = 1 ( −τ& + v cos θ e cosψ e ) + 1 v cos θ e sinψ e − 1 v sin θ e c3 c1 c1 c1

)

(u

θ

(

)

− δ&θ +

ψ e − δψ c3

)

(u

ψ

)

− δ&ψ +

y1 y z z v cos θ e sin δψ − 1 v cos θ e sin δψ − 1 v sin δθ + 1 v sin δθ c1 c1 c1 c1

Let τ& , uθ and uψ be defined by (16). Then

(ψ e − δψ ) + y1 v cos θ sin δ − z1 v sin δ (θ − δ ) s2 V& = − K1 1 − K 2 e θ − K 3 e ψ θ c1 c2 c3 c1 c1 2

2

= − K1

(θ − δ ) s − K 2 e θ − K3 c1 c2 2

2 1

(ψ e − δψ ) c3

thus proving attractivity to the path for any initial condition that guarantees that θ e
0, ω > 0. Then following the

argument above there exists a non-zero vector p = p1 + jp2 such that

( (α

2

)

− ω 2 ) A + α B + C p2 + ( 2αω A + Bω ) p1 = 0