Tour Planning for an Unmanned Air Vehicle under Wind Conditions

Transcription

1 Tour Planning or an Unmanned Air Vehicle under Wind Conditions Rachelle L. McNeely and Ram V. Iyer Texas Tech University, Lubbock, TX and Phillip R. Chandler U. S. Air Force Research Laboratory, Wright Patterson Air Force Base, Ohio Graduate Student, Department o Mathematics and Statistics. E mail: rachelle.mcneely@ttu.edu; Phone: ext. 258; Fax: Assistant Proessor, Department o Mathematics and Statistics. E mail: ram.iyer@ttu.edu; Phone: ext. 239; Fax: Senior Aerospace Engineer. E mail: phillip.chandler@wpab.a.mil; Phone: ; Fax: o 33

2 A very important sub-problem in the task assignment problem or unmanned air vehicles (UAVs) is the evaluation o costs or the state transitions o a directed graph. Usually a Dubins vehicle lying in the absence o wind is considered in the computation o costs. However, when a prevailing wind vector ield is considered, the costs taken on very dierent values and the task assignment problem can have very dierent solutions. In this paper, we consider the problem o constructing minimum time trajectories or a Dubins vehicle in the presence o a time varying wind vector ield. We present results on the existence and uniqueness o minimum-time solutions or a Dubins vehicle lying in a general time-varying wind vector ield under some technical conditions. These results extend the conclusions o the well-known Dubins theorem. We also propose an algorithm or obtaining the minimumtime solution or an UAV and prove its convergence. We also present the results o numerical experiments that show that the importance o considering wind vector ields while planning the tour or an UAV. 2 o 33

3 Nomenclature UAV Unmanned Air Vehicle MAV Micro Air Vehicle, usually with wing span approximately 1 t. V Constant Speed o the MAV. t Time in seconds. t Free inal time or the trajectory planning problem. x Position o the center o mass o the MAV in IR 2. ψ Heading o the MAV (in radians). q = (x, ψ). (x 0, ψ 0 ) Initial position and heading at initial time 0. (x, ψ d ) Desired position and heading at inal time t. u( ) Steering control input to the MAV. u max Upper bound on u(t) or all t. W (, ) The wind vector ield is a unction o x and t in general. φ Angle by which the initial coordinate system is rotated or numerical stability o the trajectory planning algorithm. z Position o the MAV ater a coordinate transormation or numerical stability. θ Heading o the MAV ater a coordinate transormation. 3 o 33

4 Introduction Cooperative Control o multiple, autonomous unmanned air vehicles (UAVs) is an active area o research that holds enormous potential or military and civilian applications. 1 3 This new paradigm or control has been implemented in the MultiUAV simulation platorm by the Air Force Research Laboratory. 3 It has a hierarchical architecture, where at the highest level the dynamics o the controlled agents are usually suppressed and a task allocation or the agents is perormed using graph theory. The tasks or the agents are usually tightly coupled in time 3, 4 and hence estimates o the times taken or each agent to ly rom one destination to the next is highly critical or correct assignment o tasks. This estimate o times is usually obtained by considering a kinematic model o the air vehicle, along with kinematic turn-radius constraints, to keep the computation time at a manageable level. 5 7 The key result that is used in this computation is Dubins result on the existence o minimum time solutions or a kinematic model with minimum turn-radius constraint. 8 However, this result is only or valid or zero-wind and hence all o the available cost estimation algorithms are only valid or zero-wind. In this paper, we propose a new method or the computation o minimum time solutions or the trajectory planning problem or a Micro Air Vehicle (MAV) or UAVs in the presence o time-varying wind, and prove its convergence. We also present the results o numerical experiments that show that the order in which targets need to be over-lown to minimize total light time can be very dierent i the winds are accounted or. We model an MAV lying with a constant speed in the wind axes and at a constant altitude. The kinematic equations o motion or the MAV are: ẋ = V (cos ψ, sin ψ) + W (x, t); ψ = u. (1) where x = (x 1, x 2 ), W (x, t) = (W 1 (x, t), W 2 (x, t)), and W i (x, t); i = 1, 2 are unctions with bounded derivatives. These equations contain the wind vector ield that is not considered in earlier works. 5 9 O course, the actual model or a MAV consists o Euler s equations 10 and kinematic model used above implies a perect response to the turn commands. However, it must be kept in mind that the kinematic model is only employed to compute the order in which the targets are to be visited. Once the order is determined, optimal control theory should be employed to determine the actual time-optimal path to be lown by the aircrat. A major reason or using the kinematic model is the act that only necessary conditions or optimality exist or the second order model (given by Pontryagin s Maximum Principle) and no results on existence o solutions are available. The situation is dierent or the kinematic model where in the absence o wind (that is W (x, t) = (0, 0)), the well-known Dubins theorem 8 posits the existence o a time-optimal solution or any combination o initial and 4 o 33

5 inal positions and velocities o the UAV or MAV. Contemporaneously during the development o the results in this manuscript, McGee, Spry and Hedrick 11 have addressed the problem o trajectory planning in the presence o constant wind. The mathematical method employed by them appears similar to ours or the case o constant wind. They classiy the set o initial and inal states into Types 1 and 2, where Type 1 states have a minimum time solution while Type 2 states don t. Here we prove that their Type 2 states orm a set o measure zero which means that they are sparse in the set o all possible states. In our earlier work, 12 we addressed the more general case o time and space-varying winds and proved the existence o time-optimal solutions (or almost every initial and inal states) under technical conditions involving the gradients o the wind. The veriication o these conditions can be used as the starting point or a numerical algorithm to compute the time-optimal trajectory. Here, we consider time-varying wind vector ields (that is W (x, t) = W (t) or all x IR 2 and any t IR) and prove the convergence o an algorithm or computing the minimum-time solutions. generalization o the work o McGee, Spry and Hedrick. 11 Thereore, the work in this manuscript is a Getting back to the equations o motion, let q = (x, ψ). The optimal control problem to be solved can be ormulated in two ways: Problem 1. For the system (1), minimize the inal time t subject to the constraints: q(0) = q 0 = (x 0, ψ 0 ) and q(t ) = q = (x, ψ d ); u( ) a Lebesgue measurable unction, and satisies, u(t) u max = V R min, (2) or all t. In (2), R min is the minimum turn radius in the absence o wind and arises due to mechanical limitations on the aircrat. As an MAV is designed to ly between the streets while searching or targets, we must ensure that the MAV enters a street along its center line, or at least parallel to it. In our numerical examples, ψ d is the angle that the centerline makes with the x-axis o the coordinate system. Due to the wind vector ield, the actual tangent line to the solution o Problem 1 would make an angle: ẋ(t ) = arctan ( ) ( ) V sin ψ(t ) + W 2 (x, t ) V sin ψd + W 2 (x, t ) = arctan, (3) V cos ψ(t ) + W 1 (x, t ) V cos ψ d + W 1 (x, t ) which could result in the loss o the MAV. Hence, it is clear that one must change the end condition to require that ẋ(t ) = ψ d to minimize the possibility o aircrat loss. We denote ψ(t ) by ψ or simplicity and also denote W 1 (x, t ) = W (x, t ) 2 cos θ W (x, t ) and W 2 (x, t ) = W (x, t ) 2 sin θ W (x, t ). The ollowing computations yield a ormula or 5 o 33

6 ψ : V sin ψ + W 2 (x, t ) = tan ψ d (4) V cos ψ(t ) + W 1 (x, t ) V sin ψ + W (x, t ) 2 sin θ W (x, t ) = sin ψ d (5) V cos ψ + W (x, t ) 2 cos θ W (x, t ) cos ψ d V sin(ψ ψ d ) = W (x, t ) 2 sin(ψ d θ W (x, t )), (6) which leads to: ( W (x, t ) 2 ψ = arcsin V ) sin(ψ d θ W (x, t )) + ψ d ; (7) where This leads us to Problem 2: θ W (x, t ) = arctan W 2(x, t ) W 1 (x, t ) Problem 2. For the system (1), minimize t subject to the constraints: q(0) = q 0 = (x 0, ψ 0 ); x(t ) = x ; and ẋ(t ) = ψ d ; u( ) a Lebesgue measurable unction, and satisies (2). In this paper, we will ocus on the special case W (x, t) = W (t). An existence o minimumtime solution result or the more general case o time and space varying winds (or Problem 1) can be ound in McNeely. 12 In the same work, we proved the uniqueness o minimum-time solutions or almost every combination o initial and inal states or Problem 1. Here, we prove existence and uniqueness o minimum-time solutions or the case o time-varying wind vector ields (that is, W (x, t) = W (t)) or Problem 2. In other related work, Lissaman 13 and Zhao and Qi 14 discuss the use o wind gradients to do work or the vehicle while in light. Another study by Venkataramanan and Dogan is based upon the use o a nonlinear controller when reconiguring the ormation o a group o UAVs and how wind eects are taken into consideration. 15 (8) Studies by Boyle and Chamito 16 are based on a stepwise solution to a constrained optimization problem which solves a nonlinear two-point boundary value problem at each timestep. The Tour Problem In the considered mission, a small unmanned air vehicle (SUAV) gives the MAV a set o N targets x i = (x i1, x i2 ); i = 1,, N to ly over. The case in which the sensor is located on the aircrat such that a straight line light along the center-line must be altered in order to view the target, is outside the scope o this paper though it is an interesting problem in its own right. For each target, the street where it is located is ound on a map which yields two possible angles ψ i or ψ i + π or lying over the target. The mid-line o that 6 o 33

7 street is ound and the point x i is projected perpendicularly rom the target onto that line. To simpliy notation, we will represent this new point also as x i and continue to work with these coordinates henceorth. For the i-th target, two way-points x 1 i, x 2 i ); are created equally spaced along the street about the projected point. These are considered entrance/exit points or viewing the target. Thus, each exit point is described by coordinates q j i = (xj i1, xj i2, ψj i ), where i = 1, 2,, N and ψ j i is chosen to be one o the two angles ψ i or ψ i +π, such that the unit vector (cos ψ j i, sin ψj i ) points away rom the target (x i, y i ) at the point x i. The objective is to ly rom target i to target k i by lying rom a point, q j i to qk l. Thus there are our possible paths corresponding to j, l = 1, 2. In this paper, we do not address the problem o lying between q r i to q s i (that is lying over the target x i ) where r s, and assume it to be a simple straight line along the street. Once the cost o lying the optimal path between q j i to qk l or all i, k = 1, 2,, N; i k and j, l = 1, 2 then one can set up a task assignment problem as outlined in. 3 Discussion o Dubins Theorem or the zero wind case We briely discuss Dubins result on the existence and uniqueness o minimum length solutions or a particle ollowing a Lipschitz continuous path at a constant speed V, with a bound on the maximum o the curvature given by 1 R min. This theorem states that or every initial, inal positions and velocities the minimum time (or minimum length) solution is an arc-line-arc or arc-arc-arc curve. Here, by arc we mean a portion o a circle or radius R min and line means a straight line segment. Furthermore, or the arc-arc-arc solution, Dubins theorem states that the middle arc must have length greater than π R min. For the Tour planning problem, Dubins theorem 8 posits the existence o a solution to the minimum time optimal control problem or the special case W (x, t) = (0, 0) or all x IR 2 and t IR +. As the minimum time solution is invariant with respect to translations and rotations o the coordinate axis, we can change coordinates so that the initial position is at the origin o IR 2 and the inal position is along the x axis. The initial and inal velocity directions measured with respect to the x-axis are termed φ 0 and φ respectively in Figures 1-2. The direction o the velocity induces an orientation on the circles tangent to the velocity vector. In the igures, we denote the center o the counterclockwise oriented circle by Z 0 and Z respectively, while the centers o the clockwise oriented curves are denoted by Y 0 and Y respectively. Thus we can distinguish between the Z 0 LY rom the Y 0 LZ arc-line-arc solutions etc. The arc-line-arc solution is ound when the initial and inal positions are airly separated as in Figure 1, while the arc-arc-arc solution is usually ound when the initial and inal positions are close enough as in Figure 2. l = 0 in Figure 2 means that the distance between the initial and inal positions is 0. Figure 2(a) shows a Y 0 Z Y arc-line-arc curve 7 o 33

8 connecting the initial and inal velocities. Though the minimum time solutions exist, they need not be unique as seen in Figures 1(b) and 2(b). A more important point rom the point o view o numerical methods is the dierentiability o the arc-lengths as a unction o the inal positions and velocities while holding the initial positions and velocities ixed. This issue is discussed in 12 in detail. The inal result is that there is a set o inal positions and velocities that has a measure zero, and where the minimum time curves change rom arc-line-arc to arc-arc-arc type or vice-versa and at these points the arc length unction is discontinuous and hence non-dierentiable. It is also possible or arc-line-arc curves to change to arc-line-arc curves in a discontinuous manner. Time optimal trajectories in the presence o wind For a non-zero time-varying (Caratheodory) wind vector ield, it is possible to show existence o a time-optimal solution i we make the ollowing assumptions. Suppose that W is a dierentiable unction o t with bounded derivative and urthermore: A1. W = sup t W (t) < V. A2. W t β V 2 W 2 u max, where: W t = sup t W (t) t 2, and 0 < β < 1 is some constant. Remarks: The assumption A1 is easy to explain. It is needed to ensure that the MAV will have non-zero orward velocity in the presence o the worst-case head wind. The assumption A2 is needed because the MAV has a limitation on the maximum rate o turn given by inequality (2) and hence cannot keep up with very rapid large changes in the wind direction (please reer to McNeely 12 or more details). Solution to Problem 1 In 12 we proved the ollowing theorem (the statement has been modiied rom 12 to include two results proved in the same manuscript): Theorem 1 Suppose that W (t) is a time-varying (Carathéodory) vector ield o wind velocities satisying assumptions A1 - A2. Then there exists a solution to Problem 1 or the system (1). The solution is unique or almost every collection o initial and inal states. Suppose the wind W (x, t) = W (t) depends only on the time variable, while satisying the conditions (A1) - (A2). Then, we can the transorm coordinates (with the same time variable) as ollows: x = ϕ(x, t) = x t 0 W (s) ds and ψ = ψ. (9) 8 o 33

9 Let q(t) = (x(t), ψ(t)) be the solution to (1) with initial condition q 0. Ater coordinate transormation, this solution becomes: x(t) = x(t) t 0 W (s) ds and ψ(t) = ψ(t). (10) Thereore, in the new coordinates ( x, ψ), the system equations take on the orm: x = V (cos ψ, sin ψ), x(0) = x 0 ; (11) ψ = u, ψ(0) = ψ0, (12) which is o the orm considered by Dubins. The inal states o the transormed system or Problem 1 are given by: x(t ) = x t 0 W (s) ds, ψ(t ) = ψ d. (13) By Theorem 1, the time optimal solution exists or all initial and inal conditions, and is unique or almost every set o initial and inal conditions or the original system. Clearly, the optimal trajectory must be a Dubin s solution or the transormed system with initial states ( x, ψ)(0) and inal states ( x, ψ)(t ) (see McNeely 12 or a uller discussion). The problem is that t is unknown and hence x(t ) is also unknown. In the algorithm in Theorem 2, we make an initial guess or t and improve it using Newton s method. The convergence o the sequence o inal times is proved. Theorem 2 Select τ 0 > 0. For n = 1, 2,, consider the inal states: τ n 1 x(τ n 1 ) = x W (s) ds; 0 ψ(τ n 1 ) = ψ d. (14) Let L(τ n 1 ) be the length o the minimum-time Dubins trajectory with initial state ( x, ψ)(0) n 1 and inal states ( x, ψ)(τ ), and denote Set τ n T (τ n 1 ) = = τ n 1 τ n 1 L(τ n 1 ) V T (τ n 1 ). (15) 1 T (τ n 1 ). (16) Then, the sequence {τ n; n Z} converges to a unique τ > 0 or almost every set o initial and inal conditions or the original system provided τ 0 is suiciently close to τ. 9 o 33

10 Proo: Consider the unction (τ) = τ T (τ). The equation (τ) = 0 has a unique solution τ by Theorem 1 or almost every set o initial and inal states. This is because (τ ) = 0 yields τ = L(τ ). This means that the minimum-time trajectory rom x(0) to the point V x τ 0 W (s) ds takes exactly time τ or, in other words x(τ ) = x τ 0 W (s) ds. The existence and uniqueness o τ having been addressed, lets consider the dierentiability o the unction L(τ). As studied in McNeely, 12 this unction is continuously dierentiable or almost every choice o τ. Hence, provided the initial selection τ 0 is suiciently close19 to τ, Newton s method is applicable to the solution o (τ) = 0 which results in (16). Remark: Unortunately, it is diicult to characterize how close the initial selection τ 0 needs to be to τ as this depends on x 0, x, ψ 0, ψ and the wind vectorield W (t) in a complicated way. Solution to Problem 2. We will irst prove the existence o time-optimal solutions or the special case o time-varying wind W (x, t) = W (t). The proo or the general case also ollows the same pattern though the equations look more complicated. As the algorithm in this paper is designed or the case or W (x, t) = W (t), this is the signiicant case or our purposes. We have the ollowing theorem: Theorem 3 Suppose that W (x, t) = W (t) is a time-varying (Carathéodory) vector ield o wind velocities satisying assumptions A1 - A2. Then there exists a solution to Problem 2 or the system (1), and the solution is unique or almost every collection o initial and inal states. Proo: Existence: The proo o existence proceeds in similar ashion to the proo o Theorem 1 as given in. 12 The idea is to establish one solution and then use Filippov s theorem 12, 18 to prove the existence o a minimum-time solution. The worst case wind vector-ield is the one that yields the system equations: ẋ = (V + W ) (cos ψ + sin ψ); ψ = u, (17) ẋ max θ as the instantaneous turn-radius given by R min(t) V + W = = R V min is the largest possible. The condition on ψ(t ) becomes: ψ(t ) = ψ d. Dubin s Theorem 8 is applicable to system (17) with initial states (x 0, ψ 0 ) and inal states (x, ψ d ), and it yields the existence o a minimum-time solution or the worst-case wind. For a given wind ield W (t), one can construct a piecewise continuous u(t) such that the solution stays on the trajectory or the worst-case wind as shown in the proo o Theorem 1 in McNeely. 12 This solution satisies ẋ(t ) = ψ d or some t > 0, that is exactly what was desired. However, this solution is 10 o 33

11 not the minimum time optimal solution or the wind W (t), and we prove the existence o the latter using Filippov s theorem. In order to apply the theorem, we need to make a change o variables. Let α(t) = arctan Then we obtain (the variable t is suppressed or brevity): ( ) V sin ψ(t) + W2 (t). (18) V cos ψ(t) + W 1 (t) α = (V cos ψ + W 1)(V u cos ψ + Ẇ2) (V sin ψ + W 2 )( V u sin ψ + Ẇ1) V 2 + W V (W 2 sin ψ + W 1 cos ψ) = V 2 u + V u (W 2 sin ψ + W 1 cos ψ) + V (Ẇ2 cos ψ Ẇ1 sin ψ) + (W 1 Ẇ 2 W 2 Ẇ 1 ) (20) V 2 + W V (W 2 sin ψ + W 1 cos ψ) The above equation can be written entirely in terms o α(t) using the equations: ( W (t) 2 ψ(t) = arcsin V (19) ) sin(α(t) θ W (t)) + α(t); θ W (t) = arctan W 2(t) W 1 (t). (21) The system in variables (x, α) has initial and inal conditions as ollows: x(0) = x 0 ; x(t ) = x (22) ( ) V sin ψ0 + W 2 (0) α(0) = arctan ; α(t ) = ψ d (23) V cos ψ 0 + W 1 (0) It is straightorward to see that the system in the variables (x, α) satisies all the conditions o Filippov s theorem. 12, 18 As we have already shown the existence o one solution that satisies the initial and inal conditions, there exists a measurable control u ( ) and a minimum-time solution x ( ), α ( ) that satisies the boundary conditions or almost all initial and inal states. Uniqueness: To show the uniqueness o the solution or almost all initial and inal states, we need to work with the x, ψ coordinates as in (9): x = V (cos ψ, sin ψ), x(0) = x 0, x(t ) = x W (s) ds; (24) 0 ( ) V sin ψ(t ) + W 2 (t ) ψ = u, ψ(0) = ψ0, arctan V cos ψ(t = ψ d. (25) ) + W 1 (t ) By our proo o existence, we know that there exists a minimum time solution or almost all initial and inal states or the original system. Assuming it exists, let (x, ψ)(t), t [0, t ] be this solution. Denoting x(t ) by x and ψ(t ) by ψ, the inal states in (24) and (25) can be t 11 o 33

12 written as: t x = x W (s) ds; (26) 0 ( ) W (t ) 2 ψ = arcsin sin(ψ d θ W (t )) + ψ d ; θ W (t ) = arctan W 2(t ) V W 1 (t ). (27) With ( x, ψ ) as the desired inal state or the transormed system, we see that Dubins Theorem 8 applies and there exists a unique solution ( x, ψ )( ) deined on [0, t ] or almost every combination o initial and inal states o the transormed system, where t t. We can transorm the solution back to the original coordinates (x, ψ ) while remembering that: x (t ) = x, ψ (t ) = ψ, (28) to obtain: x (t ) = x (t ) + t t 0 W (s) ds; (29) = x + W (s) ds; (30) t ψ (t ) = ψ. (31) A suicient condition or x (t ) = x and ψ (t ) = ψ is t = t. I t < t, then it ollows that both solutions (x, ψ)(t), t [0, t ] and (x, ψ )(t), t [0, t ] satisy the boundary conditions, and the irst solution is not the minimum-time solution contradicting our original assumption. Hence t = t is both necessary and suicient or x (t ) = x and ψ (t ) = ψ. As the Dubins solution in the transormed coordinates is unique or almost every choice o end conditions, the same is true or the minimum time solution in the original coordinates as well. The same algorithm and convergence proo or Problem 1 as summarized in Theorem 2 also applies to Problem 2 with a minor change. 12 o 33

13 Theorem 4 Select τ 0 > 0. For n = 1, 2,, consider the inal states: τ n 1 x(τ n 1 ) = x W (s) ds; (32) 0 ( ) W (τ n 1 ψ(τ n 1 ) 2 ) = arcsin sin(ψ d θ W (τ n 1 )) + ψ d ; where (33) V θ W (τ n 1 ) ) = arctan W 2(τ n 1 W 1 (τ n 1 ). (34) Let L(τ n 1 ) be the length o the minimum-time Dubins trajectory with initial state ( x, ψ)(0) n 1 and inal states ( x, ψ)(τ ), and denote Set τ n T (τ n 1 ) = = τ n 1 τ n 1 L(τ n 1 ) V T (τ n 1 ). (35) 1 T (τ n 1 ). (36) Then, the sequence {τ n; n Z} converges to a unique τ > 0 or almost every set o initial and inal conditions or the original system provided τ 0 is suiciently close to τ. Proo: The proo is identical to that o Theorem 2, except or the act that Theorem 3 is invoked in the proo instead o Theorem 1. Next, we describe the algorithms given in Theorem 2 or Problem 1, and Theorem 4 or Problem 2 in greater detail. In Step 2, we change coordinates or numerical stability rom (x, ψ) to (z, θ) such that in the new coordinates z 0 is at the origin and z is along the positive z 1 -axis. One could also employ a scaling o the axes in this step. In Step 3, we set the wind to zero and use Dubins method to obtain an initial estimate τ 0 or the inal time. In Step 4, we modiy the inal angle θ d to θ or Problem 2. This step is not necessary or Problem 1. At Step n, the estimate to t is τ n, and yields θn. On convergence o the algorithm the correct value or θ will be ound along with t. Algorithm or time optimal paths in the presence o time-varying wind 1. Start Input q 0 = (x 10, x 20, ψ 0 ) and q = (x 1, x 2, ψ d ) (ψ d is the desired inal angle), minimum turn radius (R min ), speed (V ), and time-varying wind velocity (W (t)). Select a tolerance ɛ > o 33

14 2. Perorm a coordinate transormation or numerical stability Let cos φ = x 1 x 10 and sin φ = x 2 x 20 x x 0 x x 0 [ ] cos φ sin φ Deine: R = sin φ cos φ We use the rotation matrix R and a shit o origin so that (x, ψ) is transormed into (z, θ), with z along the positive z 1 -axis. Speciically, z = R T (x x 0 ), and θ = ψ φ. We have to consider 4 cases to compute φ in an actual implementation: (a) I cos φ = 0 and sin φ > 0 let φ = π/2 (b) I cos φ = 0 and sin φ < 0 let φ = π/2 (c) I cos φ > 0 let φ = arctan sin φ cos φ (d) Else φ = arctan sin φ cos φ + π Denote: θ 0 = ψ 0 φ and θ d = ψ d φ. z 0 and z are computed according to the coordinate transorm: z 0 = (0, 0); z = R T (x x 0 ). The wind vector ield W (t) is transormed to W (t) = R T W (t). The new system o equations are: ż = V (cos θ, sin θ) + W (t); θ = u. (37) 3. Step 0: Use Dubins method to ind an initial value or τ 0 Use (z 0, θ 0 ) and (z, θ ) to compute a Dubins trajectory or the transormed system with zero wind. Let L be the length o this trajectory. Set τ 0 = L V. 4. Step n: Find the angle θ d Find θ dependent on θ d, V, and W (τ n ). Figure 3 describes the idea behind the computation. Denote by V = ( V 1, V 2 ) the vector V (cos θ, sin θ ). At t = τ n, Consider V + W (τ n) = ( V 1 + W 1 (τ n), V 2 + W 2 (τ n)) where V is as yet unknown (because τ n is unknown). Note that tan θ d = V 2 + W 2 (τ n ) V 1 + W 1 (τ n ). Thereore V 1 + W 1 (τ n ) = V + W (τ n ) cos θ d (38) V 2 + W 2 (τ n ) = V + W (τ n ) sin θ d (39) 14 o 33

15 I we combine these equations and solve or V 2, we have the ollowing equation. V 2 = ( V 1 + W 1 (τ n )) tan θ d W 2 (τ n ) (40) We also have: V V 2 2 = V 2 (41) Solve or V 1 and V 2 using equations (40) and (41). As cos(θ ) = V 1 V and sin(θ ) = V 2 V, we ind θ using the ollowing process: (a) I cos θ = 0 and sin θ > 0 let θ = π/2 (b) I cos θ = 0 and sin θ < 0 let θ = π/2 (c) I cos θ > 0 let θ = arctan sin θ cos θ (d) Else θ = arctan sin θ cos θ + π 5. Step n continued: Compute τ n Let z = z τ (n 1) 0 state ( z, θ n 1 it is a unction o τ (n 1). W (s) ds. Use Dubins method with initial state (z 0, θ 0 ) and inal ). Denote the length o the path divided by the speed V by T (τ (n 1) ) as 6. I T τ (n 1) < ɛ, then go to Item 7. Otherwise, compute the solution τ n T (τ) τ using Newton s Method or Modiied Newton s Method. 19 τ n = τ n 1 The derivative term in the denominator is computed numerically. Go to Item Stop We take τ n as the approximate value o τ n. to (τ) = n 1 (τ ). (τ n 1 ) Remark: For the case o constant wind W (t) = W, the algorithm is particularly simple to implement and intuitive. It can be interpreted as ollows. We seek to ind a state ( x w, ψ ) such that in the absence o wind, the Dubins solution takes time t to reach this state rom ( x 0, ψ 0 ). The property o this state is that it takes exactly time t or a particle to go rom x w to x. The same interpretation also holds or the case o time-varying wind. Numerical Results To illustrate the eectiveness o the algorithm, we look at a ew numerical examples while considering a constant wind vector ield. In practice, it is not possible to have knowledge o the wind vector ield W (t) beore hand, and hence constant wind is the important case 15 o 33

16 to consider. We irst consider a simple trajectory planning problem with given initial and desired inal positions x 0, x and headings ψ 0, ψ d. We consider Problem 2 or several cases with various constant wind vector ields. For all cases, we set V = 40 m/s and R min = 100 m. The units o distance in the discussion below is meters, that o speed is meters per second, and that o angle is radians. Trajectory Planning Example We consider two generic initial and inal states, where the inal states where chosen using the random number generator in MATLAB. The wind vectors range rom W = [ 10; 15] to W = [10; 15] and all satisy the condition W 2 < V. Speciically, the states were q 0 = ( 650, 100, π/4) and q = ( 692.9, , ). The tolerance ɛ or the algorithm was chosen to be ɛ = 0.1. Applying the algorithm or Problem 2, the results shown in Figure 4 and summarized in Table 1 were obtained. Figure 4 shows the actual trajectories computed and their variation with the wind can be seen. Notice that all trajectories are tangential to each other at the end point in spite o the dierent wind conditions, which is exactly as required or solutions to Problem 2. As the wind increases rom W = [10; 10] m/s to [10; 15] m/s, a qualitative change in the trajectory can be observed. This could perhaps be explained by observing that the minimum time trajectory would be the one with the maximum coasting in the direction o the wind. Next, we consider a case where the initial and inal positions are identical - to be precise, q 0 = (0, 0, 0) and q = (0, 0, pi). It is well-known that this problem has a non-unique arcarc-arc solution or the zero-wind case and the algorithm computes one o the solutions (the clockwise one) as shown in Figure 5. The same igure also shows the solution to Problem 2 in the presence o constant wind W = [ 5; 15] m/s. Notice that now the symmetry is lost, and the solution to Problem 2 is a clockwise trajectory which is tangential to the x-axis, as required. Optimal Tour Planning In this subsection, we consider the eect o the wind on the tour planning or task assignment or MAV s. Given several target locations, an initial starting point and heading, we would like to ind the order in which to visit the targets such that the total time o travel is minimized. To do this, we irst compute exit/entry points or each target as described in the section on the Tour Problem. Then we set up a directed graph containing all possible paths between the targets themselves, and between the starting state and the targets. The minimum time taken or each state transition is computed using the algorithm and assigned as the cost o the state transition. Finally, one needs to search the cost table or the directed graph to ind a state transition sequence o minimum cost that starts with the initial state 16 o 33

17 and covers all the targets. Several publications 1, 2, 9 in the literature have addressed the search problem and that is not the topic under investigation in this paper. As our interest is in demonstrating the eect o wind on the task assignment problem, we consider two examples with 3 targets each and the direct graph search was done manually with some eort. The wind or both examples was set at [10; 15] m/s though this is just a number and the theory developed earlier does not depend on any speciic wind speeds. In the irst example, the drop o point is ( 650, 100, π ), and in the second example, 4 the drop o point is (400, 400, 5π ), and they have been denoted by S in Figures 6(a) and 4 6(b). The three targets are Target 1 ( 703, 434.9), Target 2 (790.9, 632.2), and Target 3 ( 179.5, 124.8). Note that these targets were chosen using the map o a simulated city that is available in the MultiUAV simulation. 3 The entry/exit points or each o the targets were ound as discussed in Section on the Tour Problem. Those about the irst target are 1a ( 682.9, 433.6, 0.18) and 1b ( 722.2, 440.9, 2.96). The second target has bounds 2a (810.8, 629.8, 0.19) and 2b (771.53, , ). The third target has entry/exit points 3a ( 158.5, 121.0, 0.18) and 3c ( 197.8, 113.7, 2.96). For the irst example, the Tables 2 and 3 were obtained or the state transition times, where the time is measured in seconds. The time optimal tours are plotted in Figure 6(a). The results o the minimum-time tour computation showed an improvement in the time needed to travel the minimal path in these wind conditions. The optimal path where there is no wind present was computed to be S-1b-1a-3b-3a-2b-2a which is travel rom the drop o point to the irst target, then the third target, and inally to the second target. The cost o traveling this path in the case o no wind was 69.4 seconds. In the presence o wind, the optimal path was ound to be S-1a-1b-3b-3a-2b-2a and the cost got reduced to 64.9 seconds. Although travel to each target was perormed in the same order, the approach was dierent in each scenario. This case shows how the presence o wind could help the trajectory o an MAV. For the second example, only the relationships with S and the other points change. The cost tables or this problem are given in Tables 4 and 5. The time optimal tours or the zero wind and wind case is shown in Figure 6(b). The results o the search routine showed that the minimal path in wind conditions was longer than that in no wind. The optimal path where there is no wind was S-2a-2b-3a-3b-1a-1b with cost 75 seconds. In the presence o wind, the optimal path was S-3a-3b-1a-1b-2b-2a and the cost was 92.8 seconds. This path is very dierent rom that where no wind is present. 17 o 33

18 Conclusion In this paper, we consider the problem o constructing minimum time trajectories or a Dubins vehicle in the presence o a time varying wind vectorield. We have presented results on the existence and uniqueness o minimum-time solutions or a Dubins vehicle lying in a general time-varying wind vectorield under some technical conditions. Using these results we have proposed and proved the convergence o an algorithm or obtaining minimum-time solution or such a vehicle. We have presented the results o numerical experiments that show that the importance o considering wind vectorields while planning the tour or a Dubins vehicle. Acknowledgements R. McNeely would like to acknowledge a graduate student ellowship granted by the Air Vehicles Directorate Summer Research Program, Summer R. Iyer was supported by an USAF/ASEE Summer Faculty Fellowship during Summer The authors would like to thank Pro. S. Darbha, Texas A & M university, or introducing the work o McGee, Spry and Hedrick to us in May We thank the anonymous reerees or taking the time to review the paper and giving us valuable suggestions, and this has resulted in an improvement in the quality o the manuscript. 18 o 33

19 Reerences 1 Chandler, P., and Pachter, M., Hierachical control or autonomous teams, AIAA , Proc. AIAA Guidance, Navigation and Control Conerence, Montreal, Canada, August, Schumacher, C., Chandler, P., and Rasmussen, S., Task allocation or wide area search munitions via network low optimization, AIAA , Proc. AIAA Guidance, Navigation and Control Conerence, Montreal, Canada, August, Rasmussen, S., Mitchell, J. W., Chandler, P., Schumacher, C., and Smith A. L., Introduction to the MultiUAV2 simulation and its application to cooperative control research, pp , Vol. 7, Proc. American Control Conerence, Portland, OR, June, Chaudhry, A. I., Misovec, K. M., and D Andrea, R., Low Observability Path Planning or an Unmanned Air Vehicle Using Mixed Integer Linear Programming, pp , Vol. 4, Proceedings o the 43rd IEEE Conerence on Decision and Control, Paradise Island, The Bahamas, December Yang. G., and Kapila, V., Optimal path planning or unmanned air vehicles with kinematic and tactical constraints, pp , Vol. 2, Proc. 41st IEEE Conerence on Decision and Control, pp , Las Vegas, NV, December Howlett, J., Path Planning or Sensing Multiple Targets rom an Aircrat, Masters Thesis, Department o Mechanical Engineering, Brigham Young University, December Anderson, E., Extremal Control and Unmanned Air Vehicle Trajectory Generation, Masters Thesis, Department o Electrical and Computer Engineering, Brigham Young University, April Dubins, L., E., On curves o minimal length with a constraint on average curvature and with prescribed initial and terminal positions and tangents, American Journal o Mathematics, vol. 79, 1954, pp Savla, K., Bullo, F., and Frazzoli, E., On traveling salesperson problems or Dubins vehicle: stochastic and dynamic environments, Proc. IEEE Con. on Decision and Control, Seville, Spain, pages , December Etkin, B., Dynamics o Atmospheric Flight, John Wiley & Sons, Inc., 1972, pp McGee, T., Spry, S., and Hedrick, K., Optimal path planning in a constant wind with a bounded turning rate, AIAA Guidance, Navigation and Control, McNeely, R., Trajectory Planning or Micro Air Vehicles in the Presence o Wind, Masters Thesis, Department o Mathematics and Statistics, Texas Tech University, May o 33

20 13 Lissaman, P., Wind Energy Extraction by Birds and Flight Vehicles, 43rd AIAA Aerospace Sciences Meeting and Exhibit, AIAA, Inc., Reno, NV, January 2005, AIAA Zhao, Y., and Ying, Q., Maximizing Endurance o Unmanned Aerial Vehicle Flight By Utilizing Wind Gradient, 2nd AIAA Unmanned Unlimited Systems, Technologies, and Operations-Aerospace, AIAA, Inc., San Diego, CA, September 2003, AIAA Venkataramanan, S., and Dogan, A., Nonlinear Control or Reconiguration o UAV Formation, AIAA Guidance, Navigation, and Control Conerence and Exhibit, AIAA, Inc., Austin, TX, August 2003, AIAA Boyle, D. P. and Chamito, G., E., Robust Nonlinear LASSO Control: A New Approach or Autonomous Trajectory Tracking, AIAA Guidance, Navigation, and Control Conerence and Exhibit, AIAA, Inc., Austin, TX, August 2003, AIAA Betts, J. T., Survey o Numerical Methods or Trajectory Optimization, Journal o Guidance, Control and Dynamics, Vol. 21, No. 2, March-April 1998, pp Filippov, A. F., On certain questions in the theory o optimal control, SIAM Journal o Control, Ser. A, Vol. 1, No. 1, 1962, pp Stoer, J., and Bulirsch, R., Introduction to Numerical Analysis, 2nd Ed., Springer- Verlag, New York, 1993, pp o 33

21 Table 1: Table 2: Table 3: Table 4: Table 5: List o Table Captions Wind vectors and minimum times or optimal trajectories. Tour example 1: zero wind case. Tour example 2: wind case. Tour example 2: zero wind case. Tour example 2: wind case. 21 o 33

22 Wind vector Time (m/s) (s) [ 10, 15] [ 5, 10] [0, 0] [5, 5] [10, 10] [10, 15] Table 1: 22 o 33

23 S 1a 1b 2a 2b 3a 3b S a b a b a b Table 2: 23 o 33

24 S 1a 1b 2a 2b 3a 3b S a b a b a b Table 3: 24 o 33

25 S 1a 1b 2a 2b 3a 3b S a b a b a b Table 4: 25 o 33

26 S 1a 1b 2a 2b 3a 3b S a b a b a b Table 5: 26 o 33

27 List o Figure Captions Figure 1: Illustration o arc-line-arc solutions or the minimum time optimal control problem. a): Illustration o an arc-line-arc Y 0 LZ minimum length/time solution or suiciently separated initial and inal positions. b): Illustration o non-unique arc-line-arc minimum length solutions. Figure 2: Illustration o arc-arc-arc solutions or the minimum time optimal control problem. a): Illustration o an arc-arc-arc solution when the initial and inal positions coincide. b): Illustration o non-unique arc-arc-arc solutions. Figure 3: Relationship o θ d and θ. Minimum time arc-arc-arc solutions or zero and non-zero wind con- Figure 4: ditions. Figure 5: ditions. Minimum time arc-line-arc solutions or zero and non-zero wind con- Figure 6: Minimum path examples comparing wind and no wind tours o three targets. a): Tour planning problem 1. b): Tour planning problem o 33

28 Illustration o an arc-line-arc Y 0 LZ minimum length/time solution or suiciently separated initial and inal positions. Illustration o non-unique arc-line-arc minimum length solutions. Figure 1: 28 o 33

29 Illustration o an arc-arc-arc solution when the initial and inal positions coincide. Illustration o non-unique arc-arc-arc solutions. Figure 2: 29 o 33

30 Figure 3: 30 o 33

31 Figure 4: 31 o 33

32 Figure 5: 32 o 33

33 Tour planning problem 1. Tour planning problem 2. Figure 6: 33 o 33