AIAA Gudance, avgaton, and Control Conference 2-5 August 200, Toronto, Ontaro Canada AIAA 200-8437 Cooperatve UAV Trajectory Plannng wth Multple Dynamc Targets Zhenshen Qu and Xangmng X 2 Harbn Insttute of Technology, Harbn, 50080, Chna Anound Grard 3 Unversty of Mchgan, Ann Arbor, MI, 4809 The paper addresses the problem of multple UAV trajectory plannng wth dynamc targets. The problem s studed under the MILP framework, where how to express the nonlnear tme-dependent cost functon between two targets n a lnear form makes the key dffcultes. To solve the problem, the cost functon between two nodes s determned usng propotonal gudance law to acheve shortest chasng tme, then t s lnearzed wth nonunform segmented tme ntervals to keep the problem solvable wth MILP. To process the problem wth obstacle avodance, addtonal tme ntervals correspondng to blocked obstacle regons are ntroduced nto the cost functon. Target leavng tme decson varable values fallen n the ntervals are treated as nfeasble by ntroducng new logc decson varables. Varous smulaton examples verfy the proposed method. B c j n t j x j v v p omenclature = a large enough number used n MILP constrants = flyng tme from target to target j; n the dynamc target case t s tme-dependent = number of UAVs = number of targets = total number of obstacle polygon vertces = the tme the UAV leavng target j = 0- decson varable = target velocty = UAV speed I. Introducton lthough much effort has been made regardng the problem of cooperatve UAV trajectory plannng wth statc Atargets, research amng at dynamc targets seems rare. The mxed nteger lnear programmng (MILP) based approaches consttute an mportant class of solutons, -5 due to the effcent software mplementaton and global convergence features of MILP algorthms. Among these, a general desgn scheme s to mnmze the cost functon contanng performance (usually mnmum tme) and destnaton reachng decson varables, and the constrant part ncludes arcraft dynamcs, obstacle avodance and other constrants. These methods have been so mature that they can work well n complex condtons where multple UAVs cooperaton and mscellaneous task constrants are consdered. The problem scale n these methods depends heavly on the arcraft dynamc state equaton constrants, and a long plannng horzon wll generate too much decson varables to be mplemented n nowadays lnear programmng software. To deal wth the problem, the rollng optmzaton technques are usually adopted, such as recedng horzon control and predctve control.,2 However, the selecton of proper tme horzon arouses the new Asscocate professor, Department of Control Scence and Engneerng, Room 403, Bldg E2, 2 Ykuang St., Harbn 50080, Chna; mraland@ht.edu.cn 2 Undergraduate student, Department of Control Scence and Engneerng, Room 304, Bldg E2, 2 Ykuang St., Harbn 50080, Chna; 623222600@qq.com 3 Assstant professor, Department of Aerospace Engneerng, 3049 FXB, Ann Arbor, MI 4809. AIAA member; anouck@umch.edu Copyrght 200 by the, Inc. All rghts reserved.
problem. Too short horzon wll produce less optmzed result, and long horzon wll ncrease the problem scale. Obvously, the underlyng complexty of ths problem arses from the nherent couplng between the task assgnment and the trajectory generaton. Therefore, Ref. 3 proposed to decompose these two problems to gan much faster computaton. Followng the dea, task assgnment becomes the key problem, 4-6 for the trajectory plannng after defnte task assgnment s a common problem and can be solved easly usng MILP. However, most exstng results only consder statc targets, and the tme-varyng features of the targets prevent these methods to be extended to dynamc target stuaton. Compared wth the stuaton where all targets are statc, the dynamc verson s fundamentally a tme-dependent TSP problem and dffcult to solve. Good approxmaton results have been acheved only n very lmted cases. 7-8 In recent years some researchers try to address the problem usng ntellgent optmzaton methods. Ref. 9 solves the problem wth genetc algorthm, under the assumpton that relatve to the UAVs the targets are approxmately fxed n poston. Ref. 0 consders a more complex case wth more constrants usng PSO, however all vehcles n the problem are assumed to travel from one destnaton to another wth the unt speed. Besdes, no global convergence to the mnmum s guaranteed n these methods. In the paper we address the problem of sngle and cooperatve UAV trajectory plannng wth multple dynamc targets, especally the task assgnment problem, under MILP framework. The key dea s the non-unformly pecewse lnearzaton of the tme cost between targets, and choosng proper constrant formulaton to ft the problem n the MILP framework. The remnder s organzed as follows: Secton II studes the cases of sngle and cooperatve UAVs for multple dynamc targets wthout obstacle avodance. Secton III extends secton II s result to the case wth obstacle avodance. Conclusons are gven n Secton IV. II. Trajectory Plannng wthout Obstacle Avodance In ths secton, we address the problem of dynamc target assgnment problem wth sngle or cooperatve UAVs when no obstacle avodance s consdered. A. Problem Formulaton Consder a sngle or a team of UAVs executng searchng and reconnassance tasks aganst multple dynamc targets, as shown n Fg.. The underlyng problem s a movng target TSP and can be descrbed as follows: Gven a set of targets G = { g, gn}, each target g movng at constant velocty v =[u, v ], and a UAV startng from the same orgn at constant speed v p, fnd the shortest tour startng and endng at the orgn, such that the vehcles vsts all targets. B UAV C A Target Target startng pont Target trajectory UAV trajectory Fgure. Demonstraton of trajectory plannng problem wth dynamc targets To establsh solvable MILP formulaton, we augment the target space as follows. Let target be the startng pont of all UAVs, and ntroduce nodes, 2,, correspondng to returnng ponts for each of the vehcles respectvely. Choosng decson varable 2
f UAV fly from node to j xj = 0 others and t j s the tme when a UAV leavng node j, usng the problem formulaton technques n Ref., the problem can be formulated n a quas-milp form: mn t n + k () s.t. n k = = ( j = 2,..., ) (2) = j = ( = 2,..., n) (3) j = 2 j n x j = (4) j = 2 t = 0 (5) (,..., ; 2,...,, ) {,..., } tj t Bxj cj B = n j = j (6) Tr Tr n (7) j j Tr Tr { 0, } (,..., ; 2,..., ) t 0 (,..., n ) x = = n j = (8) = + (9) The objectve functon () mnmzes the total flyng tme of all UAVs. Besde ths, we can also consder mnmzng the maxmal flyng tme of all UAVs as the objectve functon, as shown n (): mn max t n + j (0) j {,2,..., } Constrants (2) and (3) ensure that each target s reached once and only once. Constrants (2) and (3) ensure that each target s reached once and only once. Constrant (4) ensures that exactly UAVs are used. Constrant (5) sets the startng tme of all UAVs as the reference tme. Constrants (6) compute the leavng tme at node j, where c j s the chasng tme cost and wll be explaned n detal below. Constrant (6) and (7) work together to elmnate possble subloops. B. Lnearzaton of Tme-Dependent Flyng Cost If the flyng cost c j n (6) s constant, the problem degrades to the common TSP problem and can be solved easly usng MILP. However, for the dynamc targets the cost s obvously tme dependent. Our task s () decde the expresson of the tme cost relatve to current tme decson varable; and (2) lnearze the expresson to make the problem solvable n the MILP framework. ow consder the frst problem. Assume target and j move wth a velocty of ( vx, vy) and ( vxj, v yj), repectvely. At tme t and t j, the UAV reaches targets and j n order at poston P and T, as shown n Fg. 2. Let v p be the maxmum UAV speed, t s easy to prove that () reaches the mnmum only when the UAV flys at maxmum speed, 7 therefore we can assume t flys at constant speed v p. Accordng to the propotonal gudance law, UAV chases the target at the shortest tme only when ts velocty component along the PQ drecton s equal to target j s velocty component n that drecton. Therefore, we have PQ tj t = () v v 2 where stands for the Eucldean dstance, and v 2 and v are shown n Fg. 2. After smple geometrc calculatons we have 2 c = t t v ( ) j j p 3
= ( Δ x) + ( Δy) 2 2 2 2 ( v 2 2 )( ) ( 2 2 p vy Δ x + vp vx)( Δy) 2vxvyΔxΔy ( vxδ x+ vyδy) (2) T ψ θ ϕ v yj v j ( j, yj) Q x v v xj v ( 0, 0 ) O x y j j j C j v p v 2 O ( x, y ) 0 0 (, ) P x y v S Fgure 2. Geometrc demonstraton of dynamc flyng cost calculaton. Two targets start at postons O and O j, reach postons P and Q at tme t, and postons S and T at tme t j. UAV leaves target at tme t and reaches target j at tme t j. v 2 and v are UAV s velocty component along PQ and ts perpendcular drecton. where ( 0 0) ( ) and ( 0 0) ( ) Δ x = x x = x x + v v t (3) j j xj x Δ y = y y = y y + v v t (4) j j yj y From (2)-(4) we know c j depends only on decson varable t. Although the functon s a complex nonlnear functon, ts shape lke a quadratc functon so much. To ft nto the MILP framework, the functon needs to be lnearzed. Consder ts quadratc-lke shape where the valley s curvng and two sdes are nearly lnear, we use pecewse lnearzaton method where the t axs s adaptvely segmented nto non-unform ntervals, as shown n Fg. 3. More turnng ponts means more precse result, however ncrease the computatonal burden. In the followng smulaton the pont count s set to 6, whch proves to be reasonable n most computaton. C. Smulaton Results The smulaton was performed on PC platform wth Intel Core2 CPU and 2G memory. The MILP problem s processed usng and IBM OPL CPLEX. 3 UAVs startng pont s set to be the orgn. All targets ntal poston and ther velocty are randomzed at the begnnng of smulaton. The tme-dependent cost term s lnearzed usng SLMTools n Matlab. 4. Sngle UAV Frst a smple case was consdered: a sngle UAV wth three dynamc targets. Intal smulaton values are shown n Table, and UAV speed s set to 0. In OPL IDE, the problem was solved and the followng decson varables are gven: 0 0 0 0 0 0 X =, tme = 0 0 0 [ 0 6.697 3.903 2.289 23.375] 0 0 0 4
a) Lnearzaton wth 3 turnng ponts. Error can be b) Lnearzaton wth 6 turnng ponts. o obvous error easly observed at the second pont. can be observed. Fgure 3. Tme cost functon lnearzaton Table. Intal values for the smulaton of a sngle UAV wth three dynamc targets Target A Target B Target C Intal poston (35, 2) (-30, 20) (-0, -42) Velocty (-0.8, -4.5) (, 2.4) (-, -) The correspondng trajectory s O B C A O, as shown n Fg. 4a). The fnal mnmum value s * f = 23.375. Snce only three targets are consdered, the result can be easly verfed usng enumeratng all feasble paths. Fg. 4b) shows the result for fve dynamc targets, where the correspondng trajectory s O E C A D B O. Readers may refer to Ref. 2 for detaled parameters and verfcaton process. UAV waypont Target trajectory Target ntal poston UAV orgn a) Three dynamc targets b) Fve dynamc targets Fgure 4. Trajectory plannng results for sngle UAV case 2. Cooperatve UAVs To extend results above, multple cooperatve UAVs are now consdered. In ths case the objectve functon (0) s selected to better ft the requrement (mnmum completon tme). We frst consdered the case wth 2 UAVs and 3 targets. Intal smulaton values and UAV speed settngs are the same as n. The solved decson varables are 5
0 0 0 0 0 0 0 X =, tme = 0 0 0 0 [ 0 9.442 3.8620 4.9227 7.740 3.527] 0 0 0 0 * The fnal mnmum value s f = 3.527. Correspondng trajectores are O C A O and O B O, as shown n Fg. 5. The result has been verfed to be the correct soluton. Smulatons contanng more UAVs and targets (up to 0) were also performed and tested. 2 Fgure 5. Trajectory plannng results for 2 UAVs and 3 dynamc targets case III. Trajectory Plannng wth Obstacle Avodance ow consder the plannng problem wth obstacle avodance, as shown n Fg. 6. Here the obstacle means polygonal zones n the workspace whch the UAVs cannot traverse whereas the targets can. Therefore, t represents not only obstacles but also hazerous zones where the defence unts exst. Fgure 6. Demonstraton of trajectory plannng problem wth obstacle avodance A. MILP Formulaton Compared wth secton II, the only dfference s the ntroducton of obstacles. Observe that f a path connectng two targets s blocked by an obstacle, the feasble path between the two targets must contan one or more vertces of 6
the obstacle polygon. Therefore, by stll choosng decson varables x j and t j, the problem can be formulaton n the followng MILP form: mn max t n + + k (5) s.t. k {,2,..., } x0 j = (6) j= + x0 j = 0 (7) j= = ( j =,..., n, +,..., + ) (8) = 0 ( j =,..., ) (9) = 0 + = ( =,..., n) (20) j= x = 0 ( j =,..., ) (2) = + ( =,..., ) (22) j= + j = 0 = x = xj ( j =,..., ) (23) ( 0,... ;,..., ; ) x Tr Tr {,..., n + } tj t Bx j cj B = j = + j (24) j (25) Tr j Tr t 0 = 0 (26) xj = { 0, } ( = 0,..., ; j =,..., + ) (27) t 0 j =,..., + (28) j ( ) Smlar to secton II, constrants (6) and (7) ensure that exactly UAVs are used. Constrants (8) ensure that each target s reached once. Constrants (9) ensure that each obstacle polygon vertex s reached once at most. Constrants (20) ensure the next destnaton after reachng a target should be an unreached one. Constrants (2) ensure UAVs can t leavng a vertex and return to that vertex. Constrants (22) ensure each UAV reach any vertex at most only once and leave that vertex. Constrants (23) ensure that f a vertex s reached by a UAV, a path leavng from that vertex must exst, and vce versa. B. Revsed Cost Functon for Obstacle Avodance Although constrants (24) are the same as (6), we notce the former exert hdden constrants that any feasble path doesn t cross the obstacle. One way to meet the constrants s to add addtonal segment ntersecton detecton constrants. Unfortunately, by now we can t fnd a lnear algorthm to ft nto the above MILP formulaton. Another way s to process the cost functon c j beforehand wth addtonal ntervals correspondng to blocked tme segments. Ths method can not only meet the obstacle constrant, but save the computaton by not addng addtonal constrants n the onlne MILP optmzaton process. Let s demonstrate the method by a smple example. In Fg. 7a), the orgnal path between target A and C crosses the obstacle trangle. Represent coordnates of the two termnals of the path segment as (x, y ) and (x 3, y 3 ), then the correspondng lne equaton s: x x y y = (29) x3 x y3 y The tme that the path between target A and C reaches each vertex of the trangle can be calculated by substtutng coordnates of the vertces to (29). Therefore, the constrants can be defned as: 7
m a ( t starta && t enda) xj (30) a= where start and end are startng and endng tme of vertex s correspondng regon s crossed by the path segment, and m a s the number of tmes the segment s blocked by the obstacle polygons. In ths example m a =. The constrants means f x j =, then the value of t s not allowed to take wthn the desgnated nterval. C. Smulaton Results Stll take the example used n secton II wth a trangle obstacle. Usng proposed method we obtan the trajectory O C B D A O, as shown n Fg. 7b). For the same problem wth a rectangle obstacle, the trajectory s O B C A E O, as shown n Fg. 7c). It s shown by exhaustve evaluaton that these solutons are the optmal soluton. a) The soluton s unfeasble after addng obstacle b) Soluton trajectory usng proposed method c) Soluton trajectory usng proposed method (rectangle obstacle) Fgure 7. Trajectory plannng results for UAV and 3 dynamc targets wth obstacle avodance IV. Concluson Under the MILP framework, the paper proposes a soluton to the cooperatve UAV trajectory plannng problem wth dynamc targets. The approach mnmzes the msson completon tme or total path length, under the consderaton of multple UAVs, multple dynamc targets, and obstacle avodance. Smulaton results demonstrate the feasblty of the proposed method n varous condtons: sngle or multple UAVs, wth or wthout obstacle. Exhaustve calculaton for smple case verfes the correctness of the result. 8
Acknowledgments Zhenshen Qu would lke to thank Prof.. L. Teo n Curtn Unversty of Technology for kndly and helpful descussons. Research funded n part by grant #HIT.SRIF2009002. References Rchards, A., Trajectory Optmzaton usng Mxed-Integer Lnear Programmng, M.S. Thess, Massachusetts Inst. of Technology, Cambrdge, MA, 2002 2 uwata, Y., Real-tme Trajectory Desgn for Unmanned Aeral Vehcles usng Recedng Horzon Control, M.S. Thess, Massachusetts Inst. of Technology, Cambrdge, MA, 2003 3 Rchards, A., Bellngham J., Tllerson, M., and et al., Coordnaton and Control of Multple UAVs, AIAA Gudance, avgaton, and Control Conference and Exhbt, Monterey, CA, 2002-4588, - 4 amal, W. A., Gu., D.-W., Postlethwate, I., Real Tme Trajectory Plannng for UAVs Usng MILP, IEEE Conference on Decson and Control, Vol., Sevlle, Span, 2005, 338-3386 5 m Y., Gu D.-W., and Postlethwate I., Real-Tme Optmal Msson Schedulng and Flght Path Selecton, IEEE Transactons on Automatc Control, Vol. 52, o., 2007, pp. 9-23 6 Aldaee B., Wang H., and Landram F., A ote on Integer Programmng Formulatons of the Real-Tme Optmal Schedulng and Flght Path Selecton of UAVs, IEEE Transactons on Control Systems Technology, Vol. 7, o. 4, 2009, pp. 839-843 7 Helvg, C. S., Robns, G. and Zelkovsky, A., The Movng-Target Travelng Salesman Problem, Journal of Algorthms, Vol. 49, Issue, 2003, pp. 53 74 8 Hammar, M., and lsson,b. J. Approxmaton Results for netc Varants of TSP, Dscrete and Computatonal Geometry, Vol. 27, o. 4, 2002, pp. 635-65 9 Shma, T., and Schumacher, C., Assgnment of Cooperatng UAVs to Smultaneous Tasks usng Genetc Algorthms, AIAA Gudance, avgaton, and Control Conference and Exhbt, Vol., San Francsco, CA, 2005-5829, -4 0 Pan, F., Wang, G., and Lu., Y., A Mult-Objectve-Based on-statonary UAV Assgnment Model for Constrants Handlng usng PSO, World Summt on Genetc and Evolutonary Computaton, Vol., Shangha, Chna, 2009, 459-466 Malandrak, C. and Daskn,M. S., Tme Dependent Vehcle Routng Problems: Formulatons, Propertes and Heurstc Algorthms, Transportaton Scence, Vol. 26, o. 3, Aug. 992, pp. 85-200 2 X, X.-M., MILP Based UAV Trajectory Plannng wth Dynamc Targets, B.S. Thess, Harbn Inst. of Technology, Harbn, Chna, 200 3 IBM Corp, IBM ILOG CPLEX v2. Reference Manual, IBM, 2009 4 D'Errco, J., SLM - Shape Language Modelng, 200. 9