Path Planning and State Estimation for Unmanned Aerial Vehicles in Hostile Environments

Transcription

1 Path Plannng and State Estmaton for Unmanned Aeral Vehcles n Hostle Envronments Ran Da * and John E. Cochran, Jr. Auburn Unversty, Auburn, Alabama In ths paper, planned paths for Unmanned Aeral Vehcles (UAVs) passng through the hostle envronments wth gven startng and arrvng ponts are generated and dynamc state varables for horzontal flght are estmated. The objectve of the path plannng s to maxmze safety whle mantanng a flyable path. The procedure starts from a Vorono graph as ntal coarse path. A reference path usng parameterzed Cornu-Sprals (CSs) s calculated to refne the ntal Vorono graph. The CS curve s represented by the polynomal expresson of the path curvature. Hence, contnuty and smoothness requrements for the desgned flyable path are acheved by the contnuous curvature characterstcs of CSs. The refned path s generated by mnmzng the sum of the orthogonal dsplacements from the Vorono ponts, whle satsfyng the maxmum curvature constrants, projecton constrants and boundary constrants. A nonlnear programmng solver s used to calculate the optmzed parameters. As an example, the dynamc state varables of a UAV flyng along the reference CS path at a constant alttude are estmated usng an extended Kalman flter and smoother. I. Introducton Unmanned Aeral Vehcles (UAVs) are now wdely used n ant-terrorsm actvtes and ntellgence gatherng to enhance msson performance and maxmze safety. The susceptblty of these UAVs n hostle envronments rases requrements for flght path plannng. Path plannng strateges n hostle envronments normally are comprsed of two phases [], []. The frst phase s a Vorono graph search, whch wll generate polygonal graphs and wll * Formerly, Graduate Research Assstant, Department of Aerospace Engneerng, Auburn Unversty, Auburn, AL Currently, an Engneer at Dynamc Research, Inc., Torrance, CA, 95, avator_da@hotmal.com. Professor and Head, Department of Aerospace Engneerng, Auburn Unversty, Auburn, AL 36849, jcochran@eng.auburn.edu, Fellow AIAA.

2 optmze a safety performance ndex. The second s to use the vrtual forces emanated from the vrtual feld of each survellance radar ste to refne the generated Vorono graphs. These vrtual forces provde nformaton that can be used to reduce the vertces of the Vorono polygonal and greatly mprove the UAV performance. But, the curvature contnuty of the refned graphs, whch plays an mportant role n the stablty of the UAVs turnng maneuvers, often does not meet the requrements for a contnuously flyable path. Many knds of curves have been studed and desgned for UAVs to accomplsh ther msson [3]-[5]. Dubns curves, frst appled n robotcs path plannng, are curves along whch the UAV can move forward. Ths knd of crcle-lnecrcle curve has a jump dscontnuty n curvature at the connecton ponts between the crcle and the lne that wll cause a robot to stop at these connecton ponts when travelng through the whole path. Other curves lke those of Reeds and Shepp [6], [7] also have curvature dscontnutes at ther jont ponts. An alternatve choce, the composte clothod-lne-clothod curves, can be well desgned wth curvature beng zero at the jont ponts to elmnate the dscontnutes. Ths allows one to generate a curvature contnuous path by usng dfferent knds of smply shaped curves. Whle, under most crcumstances, we are expectng more flexblty n the curve shape that wll allow more space of change. Shanmugavel, et al. [3]-[5]. have proposed quntc Pythagorean Hodograph (PH) curves for a flyable path wth ten parameters representng each curve. The PH curves are flexble n desgn and ther curvatures are expressed n contnuous polynomals. The parameter calculaton of a PH curve s an teratve process n order to satsfy dfferent constrants. Such knd of curves can be further smplfed wth fewer parameters and a more effcent optmzaton algorthm. All the methods dscussed leave room for mprovement n the area of contnuous curvature path plannng. The Cornu-Spral (CS) [8], [9], also known as a clothod or Euler's Spral, has wde applcaton n hghway and ralroad constructon snce t can be used to desgn gradual and smooth transtons n hghway entrances or exts. Kelly and Nagy [] used a parametrc CS model to generate real-tme, nonholonomc trajectores for robotcs to mnmze the termnal posture error. Here, we consder ths CS model for use n UAV path plannng and nvestgate how ths parametrc CS curve works under dfferent constrants. To generate a flyable and safe path wth gven startng and endng ponts for UAVs passng through areas covered at least partally by several radar stes, the path constrants consdered here nclude: () mnmum accumulatve exposure to all radar stes, () contnuous curvature throughout ts length whch wll ensure flyable path, (3) maxmum curvature correspondng to the maxmum achevable lateral turn rate, (4) ntal and fnal boundary

3 constrants. Unlke other path plannng problems, ncludng that of movng objects fndng the fnal path that normally results n moton plannng or trajectory plannng wth system dynamcs [], the path consdered here s n a statc object envronment wthout dynamc constrants. The work n ths paper s based on the developed Vorono graph; t refned the graph by proposng a generalzed CS curve along wth smplfed parameter dentfcaton procedure. Most of papers on the topc of path plannng do not nclude the nformaton about dynamc state varables of UAVs flyng along the planned path. For control purposes, t s benefcal to estmate these state varables to construct entre nformaton of the flght. Kalman Flterng [] has been wdely used as an effcent tool n optmal flterng and predcton, especally n the feld of state estmaton of UAVs performng desgnated mssons [3-7]. For example, Grllo and Vtrano used an Extended Kalman Flter (EKF) to estmate the state varables and wnd velocty for a nonlnear UAV model wth Global Postonng System (GPS) measurements. Abdelkrm, et al. used an EKF and an H flter to estmate the localzaton of UAVs whose poston, velocty and atttude are measured by an Inertal Navgaton System. Campbell, et al. used two dfferent estmators to provde onlne state and parameter estmaton for path plannng n uncertan envronments. The state estmaton n these studes have a commonalty because n both cases the UAVs have sensors or other nstruments to provde useful measurement nformaton. If all of the Vorono ponts on the ntal path are fxed and expected to be followed as close as possble, they can be assumed as measurement ponts. The generated CS curve s then treated as reference soluton, so that the state varables can be estmated by the EKF. The followng sectons present the procedure for the path nformaton constructon n three parts. The frst part s the ntal rough path of the Vorono graph and the dynamc programmng search algorthm. In the second part, CS curve expresson and propertes are ntroduced and dfferent constrants and ther mathematcal expresson are explaned. Ths s followed by the systematc soluton, nonlnear programmng (NLP) solver. In the last part, the state varables are estmated based on the generated Vorono ponts and the refned reference path generated n the frst two parts. Smulaton results are presented n each part separately. II. Intal Path Generaton There are many developed graph search methods that maybe used to generate the ntal path that wll maxmze the safety of the UAVs. Two promnent ones are heurstc search [8] and probablty map search [9]. The Vorono 3

4 graph search s generally used to fnd the rough ntal path n the form of connected edges. The authors of some artcles have stated the Vorono method and the followng dynamc programmng search algorthm n detal [, ]. Here, we brefly descrbe them. A. Vorono Graph Search A Vorono dagram wll decompose a space defned by random scattered ponts nto separated cells n such a manner that each cell wll nclude one pont that s closer to all elements n ths cell than any other ponts. The graph s constructed by usng Delaunay trangulaton and ts dual graph, Vorono dagrams. The procedure of ths cell decomposton starts wth a pror knowledge of the locaton and of number of the scattered ponts assumed as radar stes here. As llustrated n Fg., the star ponts are pre-defned radar stes. The crcle ponts named as Fg.. Vorono Graph for Radar Locatons Vorono ponts are the centers of the crcumcrcle of the trangles formed by three of the Vorono stes wthout ncludng any other stes n ths crcumcrcle. The formaton of these trangles s called Delaunay trangulaton. All ponts on edges connected by the Vorono ponts have equvalent dstance to two adjacent Radar stes. By connectng all of the edges together, polygons are formed and these polygons construct Vorono graph. The UAV startng pont and endng pont are also llustrated as small trangles n the plot. If UAVs are assumed to go through the radar coverage area along the edges n the formed Vorono Graph, the probablty of exposure of each edge to radar stes can be determned usng the dstance from the ponts on the edge to the radar stes. Suppose each radar ste emts a radaton sgnal wth equvalent strength n all drectons, the threats of exposure to ths radar ste s proportonal to the nverse of dstance to the forth power. The whole edge threat cost s the accumulatve threat calculaton from the begnnng of the edge to the end. Mathematcally, t s expressed as [] x, y [( p dx x) + dy + ( p x, y x y y) ] () 4

5 where the par x, ) and x, ) are the coordnates of startng and endng ponts on the edge and p, p ) ( y ( y s the radar ste coordnate. Snce the edge poston can be expressed by the startng and endng ponts as y y = y + x x ), α = y ( x y α ( x x () so that Eq. () can be smplfed as x x [( p x x) + ( p + α y y α( x x )) ] dx (3) By assgnng a threat cost to each edge, an optmal path wth lowest probablty of detecton when passng through the radar area can be constructed. Determnng ths optmal path requres the calculaton of the threat cost for all of the avalable paths and then selects the lowest cost one after comparson. The dynamc programmng method s appled here to solve these knds of optmal path selecton problem. B. Dynamc Programmng Dynamc Programmng (DP) s a branch of Operatonal Research frst proposed by Rchard Bellman. The DP procedure s to break a problem nto many layers of subproblems and then solve each subproblem ndvdually to acheve the overall optmalty. At each layer, the optmal soluton s saved so that when we come to ths layer agan, we can retreve the saved result to avod repeated computng. In the Vorono graph of Fg., the cost of each edge s determned and the objectve s to fnd a path from the startng pont to Fg. Intal Rough Path the endng pont wth lowest total cost. Countng from the endng pont backward, at each Vorono pont, the lowest cost from that pont to the endng s computed and saved. For example, Vorono pont 5 s set wth a cost of zero and the cost from Vorono pont 4 to 5 s Ths process s repeated for all Vorono ponts. When a pont s reached that has a cost value already determned, the algorthm takes the pre-defned optmal path from that pont to the endng wthout searchng them agan. So when countng the cost at pont, t wll take the determned route from 3 to the endng pont. Fnally, the safest path s found and labeled wth dark lnes n Fg.. 5

6 III. Reference Path Generaton Obvously, the ntal path generated n Fg. s coarse and not flyable by UAVs. The fnal flght path s expected to be smooth enough everywhere so that the UAVs won t have sharp turns on the whole path. At the same tme, we also expect the path to be as close as possble to the orgnal Vorono graph to maxmze the safety factor. Snce the curvature s a key factor n determnng the smoothness of a curve, the problem turns out to be a curve fttng problem wth curvature constrants and some other constrants. The CS wth advantages ntroduced n the followng s selected as the reference path to ft the Vorono ponts and satsfy the specfed constrants. A. Cornu-Spral The CS s a well known curve whose curvature s defned as a polynomal functon of ts arc length s as n κ ( s) = α s (4) and the curvature s also the dervatve of the curve angle θ wth respect to the arc length: = κ ( s ) = dθ / ds (5) The postons of the ponts on ths curve n Cartesan coordnates are calculated by the "Fresnel Integrals": x( s) = y( s) = s s cos( θ ( s)) ds sn( θ ( s)) ds (6) One advantage of usng ths curve n solvng the path plannng problem s the smplcty of calculaton of ts arc length: t t s( t) = x ( t) + y ( t) dt = dt = t (7) The above expressons are general,.e., they apply to all forms of CS curves ncludng specal cases lke a straght lne ( n =, α ), crcle ( n =, α ) and a unt CS ( n =, α, α ). In a unt CS, the = = = = curvature equals the arc length and wll therefore ncrease wth the length. So the longer the curve, the greater the curvature. Hence, the orgn of the term spral. The coeffcents α affect the ncrease rate of the CS curve. In Fg. 3, all the CS curve curvature polynomals are order n = wth α, α vared from. to 5. It s obvous = 6

7 that when transversng the same arc length, the larger the value of α, the more the curvature ncreases. In addton, the order n the curvature expresson also affects ts shape. Fg.4 shows a seres of CS curves wth dfferent order numbers n the curvature polynomals. The coeffcents of the hghest order are all for α = wth the others set as zero. It can be seen that curves wth the hgher order polynomals curl faster than others. n α=5 α= α= α=.5 α=..8.6 n= n= n=3 n=4 n= Fg. 3. CS curves of order n = and α = wth coeffcents α changes from. to 5. Fg. 4. CS curves of α n = and other coeffcents set as zero, order n changes from to 5. Combng the two types of effects that determne the shape of the CSs, we can produce an expresson of the curvature polynomals to defne any CSs possessng as many degrees of freedom parameters as necessary to meet the requred constrants. In the problem formulaton stated above, the prmary constrant for such curves s to meet the ntal and fnal condtons whch are classfed as equalty constrants. If the ntal condtons P = x, ) are ncorporated ( y as parameters n the CS curve, at least two addtonal parameters are requred to meet the fnal boundary constrants. To have enough freedom to satsfy other nequalty constrants and coordnate all the parameters to acheve the optmzaton purpose, one more parameter s requred. Although the ncluson of more parameters n the CS expresson wll ncrease the flexblty of the curve, t wll also ncrease more calculaton burden. As a compromse between the optmalty and computaton tme, fve parameters are used n one CS curve fnally. Three of them, a, b and c, form the cubc polynomals of the curve angle expresson. θ s the ntal headng angle. The last one s the fnal arc length s f. These parameters enter the problem formulaton as follows: 7

8 x( s) = x y( s) = y 3 θ ( s) = θ + as + bs + cs κ( s) = θ ( s) = a + bs + 3cs + + s f s f cos( θ + as + bs sn( θ + as + bs 3 + cs ) ds 3 + cs ) ds (8) Then, the path plannng problem has been cast as a parameter optmzaton problem wth equalty and nequalty constrants. B. Contnuous Curvature Constrants Contnuous curvature s an mportant factor n desgnng a flyable path, because physcally the vehcle s path must be contnuous and the rate of change of curvature wth arc length s related to the drectonal command, a key nput n maneuverng UAVs. Dscontnutes n the curvature wll cause jumps n the steerng angle nput. Such dscontnuous nput s not achevable. The curvature of a planar curve defned by x (t) and (t) y s x ( t) y( t) y ( t) x( t) ( x ( t) + y ( t) ) κ ( t) = 3 / (9) The CS curve specfed n (8) wth three parameters representng ts polynomals can be expressed as κ( s ) = a + bs + 3cs () Physcally, the frst and second dervatves of curvature correspond to the lateral velocty and acceleraton of a pont (the vehcle) that s movng along the curve. Therefore, the curvature must be at least twce dfferentable to meet the contnuous velocty and acceleraton requrement. The CS curve wth curvature expresson n () has second-order polynomals, s twce dfferental and can satsfy the contnuty constrants. C. Maxmum Curvature Constrants In CS expresson of (8), the curvature s the only shape determnng factor. By usng the maxmum curvature constrant, the knematc acceleraton of UAVs can be lmted. If a proper tangental velocty s mantaned then the path desgned can actually be traversed. Snce the curvature s a parabolc functon of the arc length, ts maxmum value ponts can only occur at one of three ponts: the ntal pont, the summt or bottom pont, or the fnal pont. Mathematcally, the curvature values at those three ponts are 8

9 κ f κ = a b κ m = a 3c = a + bs + 3cs f f () If the curvature at none of these three ponts exceeds the maxmum value, then the curvature throughout the whole length wll be sutably constraned. D. Vorono Ponts Projecton Constrants In order to ft the Vorono ponts, t s necessary to mnmze the dstances from the Vorono ponts to ther projecton ponts on the CS curve. Before the parameters of the CS curve are defned, t s dffcult to calculate the orthogonal dstance, but t s obvous to see that the slope of the normal lne for the projecton pont on the CS s the negatve recprocal of ts tangent lne slope. Assume there are n Vorono ponts. The projecton of Vorono pont ( =,, n ) on the CS curve wll satsfy the followng relatonshp p p y x y( s ) tan( θ ( s )) = x( s ) () p p are the coordnates for the Vorono pont and where, ) ( x y s s the length of the CS from the startng to the correspondng projecton pont wth coordnates of x ( s ), y( s )). Instead of calculatng the projecton ponts by ( geometrc theory, they can be found by puttng the constrants on s nto Eq. (). Then, the dstance from Vorono pont to ts correspondng projecton pont s calculated as d = p x s y s )) ( x ( )) ( y ( + p (3) E. Boundary Condton Constrants The ntal and fnal condtons are the equalty constrants on the system. The path desgned by the CS curve starts from the orgnal ponts, so the ntal condton s satsfed by settng the ntal poston coordnates as parameters n the CS curve x() = x y() = y (4) 9

10 The fnal pont s determned by usng a Runge-Kutta numercal ntegraton method to satsfy the specfed fnal boundary condtons. At ths pont, all the constrants have been descrbed. Some of them can be satsfed by the propertes of the CS curve tself, others are expressed n correspondng equaltes and nequaltes. So the path plannng problem s now a parameter optmzaton problem n whch the parameters of the CS curvature expressons are dentfed whle mnmzng the accumulatve orthogonal dsplacements for all Vorono ponts. F. Nonlnear Programmng (NLP) Solver: SNOPT 6. The NLP solver used to solve the NLPP of nterest here s SNOPT [], whch s based on a Sequental Quadrature Programmng (SQP) algorthm and has been used as effcent tool to solve nonlnear optmal control problems wth constrants [-4]. SNOPT can be used to solve problems lke the followng: Mnmze a performance ndex J (x), subject to constrants on ndvdual state and/or control varables: x < x < L x U (5) constrants defned by lnear combnatons of state and/or control varables: b < Ax < L b U (6) and/or constrants defned by nonlnear functons of state and/or control varables: c < c( x) < L c U (7) Wth the above n mnd, we can transfer the path plannng problem nto a smple form. Let the NLP performance ndex J by: and the NLP varables of the CS curve as: J = n [ ( px x( s )) + ( p y y( s )) ] = (8) where (,, n) [ θ a, b, c, s,, s,, s s ] x =,, =, subject to the equalty constrants ncludng the Vorono ponts projecton constrants and boundary condtons, P and P f. These constrants wll ensure the curve fttng wll mnmze the orthogonal n f (9)

11 dsplacement and all the coordnates on the CS curve wll satsfy ts dfferental functon n trgonometrc form of Eq. (8). In addton, the nequalty constrants to ensure the flyable path can be set as: κmax a κmax b κ. max a κmax 3c κ a + bs + 3cs κ max f f max f ( s b s 3c f ) () For the Vorono sets n Fg., a CS curve to ft those ponts s calculated under two condtons, one wthout maxmum curvature constrant and the other one wth maxmum curvature constrant κ = max / 3. Ther path smulaton results are llustrated n Fg. 5 and Fg. 6, respectvely. The correspondng curvature results wth respect to curve length are llustrated n Fg. 7 and Fg. 8, respectvely. From the two knds of smulaton results, the CS curve wthout curvature constrant fts the Vorono ponts better than the one wth curvature constrant by ncreasng the lateral knematc acceleraton to follow the path. The accumulatve threat of the CS curve to the radar stes are.495 and.5 for curves wthout and wth curvature constrant, respectvely. Compared to the accumulatve threat of the Vorono graph,.439, the CS curve acheved a flyable path wth smlar level of threat exposure to the radar stes Fg. 5. CS curve wthout Curvature Constrant 5 5 Fg. 6. CS curve wth Curvature Constrant

12 Curvature κ Curvature κ Dstance s Fg. 7. Curvature Hstory wthout Constrant Dstance s Fg. 8. Curvature Hstory wth Constrant IV. State Estmaton A. UAV Model For the smplcty of llustraton, a two-dmensonal UAV nonlnear model [5] n horzontal flght s used here. Assumng all the adversaral radar stes are located n the same alttude so that the generated Vorono graph and reference path are n the lateral plane. The three degree of freedom UAV equatons of moton are lsted bellow. Ths model s based on the assumpton that the UAV s a rgd body flyng over a non-rotatng and flat Earth. X = x = u cosψ vsnψ y = u snψ + v cosψ u = rv + F x / m f ( X, U ) = v = ru + Fy / m ψ = r r = N / I zz () where (, y) x s the poston, ( u, v) s the lateral velocty components, ψ s the headng angle and r s the yaw rate. [ Fx, Fy, N] are the aerodynamc forces and moment acheved by the control actuators. m s the mass and I zz s moment of nerta about the z axs of the UAV body frame. Ths UAV model s bult n tme doman, but the poston and headng angle varables of the reference path are bult n dstance doman and ther dervatves wth respect to dstance are

13 x'( s) = cos( θ ( s)) y'( s) = sn( θ ( s)) θ '( s) = κ( s) () In order to estmate all varables n the tme doman, t s necessary to change all the varables of the reference path from dstance doman to tme doman by callng ds = V ( t) dt (3) where V (t) s the tme hstory of the velocty magntude. By substtutng ds nto Eq. (), the equatons of moton n tme doman are expressed as x ( t) = V ( t)cos( θ ( t)) y ( t) = V ( t)sn( θ ( t)) θ ( t) = V ( t) κ( t) (4) By ths transformaton, all the state varables of the UAV model can be calculated accordng to the generated reference path, whch wll then provde reference soluton for lnearzaton of the dynamc model n EKF. B. Desgn of Extended Kalman Flter and Smoother (EKFS) and Smulaton Results An EKFS [] can often make good estmates of the state varables when the measurements are under random dsturbance. By solvng the weghted least squares problem for the measurements, we can determne the most lkely values of the state varables for contnuous or dscrete systems. The procedure of EKF conssts of two parts: a predcton part to predct the current states from the last step and an updatng part wll add a correcton term to the predcted states to refne them and get more accurate values. The optmal smoother s a backward sweep procedure startng from the last tme pont back to the frst. It wll take advantage of the fltered estmates n the forward EKF and calculate the dfferences between them and the fnal smoothed values. The optmal smoothng s also a recursve computaton to further refne the estmates. Due to the precson lmtaton of the detecton nstrument, the adversaral radar stes locaton detected s under some degree of uncertanty, whch causes the subsequent uncertantes of the Vorono Ponts searched n the ntal graph. By mplementaton the above mentoned EKFS, the errors durng the measurement can be reduced and the adjustments of the state varables are estmated and added to the nomnal soluton obtaned n the EFKS algorthm. 3

14 Snce the UAV model s nonlnear, we need to derve a Jacoban matrx A by takng the dervatves of the nonlnear system equatons wth respect to the state varables. Its mathematcal form s wrtten as A = cosψ snψ r snψ cosψ r u snψ v cosψ u cosψ vsnψ v u (5) Smlarly, a Jacoban matrx B derved by makng the dervatves of the nonlnear system equatons wth respect to the control varables s B = / m / m / I zz T (6) Accordng to the observaton output, the observaton matrx H s smply defned as H = (7) These Jacoban matrces, estmated at the nomnal state ponts functon expressed as X n, are then used n the error approxmaton X = X X n = A X + B U + H. O. T (8) Then, the EKFS wll fnd an optmzed adjustment Xˆ to the nomnal soluton by gong through ts flter and smoother. Desgn of the EKFS also ncludes the selecton of the covarance matrces of the dsturbance and observaton error, Q and R, respectvely. If the measurement nose s carred out by the characterstcs of the measurement nstrument only, the elements of R can be determned from the stochastc propertes of the measurng system. Notng that the ntal and fnal poston are fxed, R at those ponts are set as extremely small numbers. The elements n Q and R drectly affect the Kalman gan K, thereafter the adjustments requre consderable effort to fnd the best ft. In ths problem, Q s a one by one dentty matrx and R s set as 4 tme a two by two 4

15 dentty matrx except the startng and endng ponts where R s.4 tmes a two by two dentty matrx. We also assume that w and v are unrelated wth zero mean values. x (m) y (m) u (m/s) v (m/s) r (rad/s) Ψ (rad) tme (sec) Fg. 9. States Hstory for reference path wthout curvature constrant (dash lne) and estmated path (sold lne) x (m) y (m) u (m/s) v (m/s) r (rad/s) Ψ (rad) tme (sec) Fg.. States Hstory for reference path wth curvature constrant (dash lne) and estmated path (sold lne) Smulatons were carred out based on two knds of reference paths generated above, one wth and the other wthout maxmum curvature constrants. The speed of the reference paths was assumed to be constant cruse speed at 5

16 maxmum weght. Calculated measurements are the poston of Vorono ponts labeled n Fg.. The startng and endng ponts are fxed measurements wth zero errors. The state varable hstory of the reference soluton and estmated soluton are plotted n Fg. 9 and Fg. for these two cases, respectvely. By addng a covarance error to the reference soluton, the estmated values further reduce the measurement errors. V. Conclusons A type of UAV paths wth contnuous curvature and constraned turnng acceleraton has been desgned usng the Vorono graph and parameterzed Cornu-Sprals (CS). The state varables for such knd of plannng path are estmated by the Extended Kalman Fler and Smoother (EKFS). The planned paths are the sub-optmal soluton of the Vorono dagram whch wll maxmze the safety factor. They refned the ntal path by usng the contnuous curvature characterstcs of the CSs and try to ft the Vorono ponts wth the mnmum orthogonal dstance. The curve fttng algorthm ncludes all parameters of CS as nonlnear programmng varables n the nonlnear programmng solver and gets the soluton n one effort wthout tedous and teratve testng to adjust the coupled varables. Ths paper also consdered dsturbances n the detecton and actng of the hostle radar stes and estmated the state varables based on the reference path generated forehead. Fnally, the three parts of ths paper: ntal coarse graph, refned reference path and state estmaton can be used to buld up a complete nformaton data set for both path plannng and control purposes. Multple UAV paths whch requre cooperatve control to ncrease smultaneous workng performance wll be consdered n the future work. References [] McLan, T. W. and Beard, R. W., Trajectory Plannng for Coordnated Rendezvous of Unmanned Ar Vehcles, AIAA Gudance, Navgaton and Control Conference, AIAA--4369, August. [] Bortoff, S. A., Path Plannng for UAVs, Amercan Control Conference, June, pp [3] Shanmugavel, M., Tsourdos, A., Zbkowsk, R., Whte, B. A., Rabbath, C.A. and Lechevn, N., A Soluton to Smultaneous Arrval of Multple UAVs Usng Pythagorean Hodograph Curves, Amercan Control Conference, June 6, pp [4] Shanmugavel, M., Tsourdos, A., Zbkowsk, R. and Whte, B. A., 3D Path Plannng for Multple UAVs Usng Pythagorean Hodograph Curves, AIAA Gudance, Navgaton and Control Conference, AIAA , August 7. 6

17 [5] Shanmugavel, M., Path Plannng of Multple Autonomous Vehcles, PhD Dssertaton, Department of Aerospace, Power and Sensors, Cranfeld Unversty, May 7. [6] Frachard, T., Scheuer, A. and Desvgne, R., From Reeds and Shepp s to Contnuous-Curvature Paths, Proceedngs of the Internatonal Conference on Advanced Robotcs, October 999, pp [7] Scheuer, A. and Frachard, T., Contnuous-curvature path plannng for car-lke vehcles, Proc. of the IEEE-RSJ Int. Conf. on Intellgent Robots and Systems, vol., September 997, pp [8] Stoer, J., Curvng Fttng Wth Clothodal Splnes, Journal of Research of the Natonal Bureau of Standards., vol. 87, no. 4, July-August 98, pp [9] Davs, T. G., Total Least-Squares Spral Curve Fttng, Journal of Surveyng Engneerng, vol. 5, no. 4, Noverber 999, pp [] Kelly, A. and Nagy, B., Reactve Nonholonomc Trajectory Generaton va Parametrc Optmal Control, The Internatonal Journal of Robotcs Ressearch, vol., no. 7-8, Jan. 3, pp [] Hsu, D., Kndel, J. C., Latombe, J. C., and Rock, S., Randomzed knodynamc moton plannng wth movng obstacles. In Proc. Workshop on Algorthmc Foundaton of Robotcs,. [] Bryson, A. E. Jr. and Ho, Y. C., Appled optmal control: optmzaton, estmaton, and control. Hemsphere Publshng Corporaton, New York, 975. [3] Grllo, C. and Vtrano, F. P., State Estmaton of a Nonlnear Unmanned Aeral Vehcle Model usng an Extended Kalman Flter, 5 th AIAA Internatonal Space Planes and Hypersonc Systems and Technologes Conference, Dayton, Oho, AIAA- 8-59, Aprl 8. [4] Abdelkrm, N., Aouf, N., Tsourdos, A. and Whte, B., Robust Nonlnear Flterng for INS/GPS UAV Localzaton, IEEE 6th Medterranean Conference on Control and Automaton, Ajacco, France, June 8. [5] Campbell, M. and Ousngsawat, J., On-lne Estmaton and Path Plannng for Mutple Vehcles n an Uncertan Envronment, AIAA Gudance, Navgaton and Control Conference and Exhbt, Monterey, CA, AIAA--4466, August. [6] Prevost, C. G., Desbens, A. and Gagnon E., Extended Kalman Flter for State Estmaton and Trajectory Predcton of a Movng Object Detected by an Unmanned Aeral Vehcle, Proceedngs of the 7 Amercan Control Conference, New York Cty, NY, July 7. [7] Kallapur, A.G. and Anavatt, S. G., UAV Lnear and Nonlnear Estmaton Usng Extended Kalman Flter, IEEE Internatonal Conference on Computatonal Intellgence for Modelng, Control and Automaton, Sydney, Australa, November, 6. [8] Korf, R. E., Real-tme heurstc search, Artfcal Intellgence, Vol. 4, No. -3, 99, pp

18 [9] Jun, M. and D'Andrea, R., "Path Plannng for Unmanned Aeral Vehcles n Uncertan and Adversaral Envronments", In Cooperatve Control: Models, Applcatons and Algorthms, Butenko, S., Murpheym, R. and Pardalos, P., Sprnger, 3. [] Holmstrom,K., Goran,A.O. and Edvall, M. M., User s Gude For TOMLAB/SNOPT, Tomlab Optmzaton Inc., 5, [] C. R. Hargraves, and S. W. Pars,, Drect Trajectory Optmzaton Usng Nonlnear Programmng and Collocaton, J. of Gudance, Control and Dynamcs, Vol., No. 4, July- August 987, pp [] Kumar, R. K., Seywald, H., and Clff, E. M., Near-Optmal Three-Dmensonal Ar-to-Ar Mssle Gudance Aganst Maneuverng Target, Journal of Gudance, Control, and Dynamcs, Vol. 8, No. 3, 995, pp [3] Betts, J. T. and Huffman, W. P., Path-Constraned Trajectory Optmzaton Usng Sparse Sequental Quadratc Programmng, Journal of Gudance, Control, and Dynamcs, Vol. 6, No., 993, pp [4] Fahroo, M. and Ross, I. M., Drect Trajectory Optmzaton by a Chebychev Pseudospectral Method, Journal of Gudance, Control, and Dynamcs, Vol. 5, No.,, pp [5] Campbell, M. E., Lee, J. W. and Scholte, E., Smulaton and Flght Test of Autonomous Arcraft Estmaton, Plannng, and Control Algorthms, Journal of Gudance, Control and Dynamcs, Vol. 3, No. 6, 7, pp