The Successive Backward Sweep Method for Optimal Control of Nonlinear Systems with Constraints

Transcription

1 AAS -6 he Sccessive Backward Sweep Mehod or Opimal Conrol o Nonlinear Sysems wih Consrains D. H. Cho * and Srinivas R. Vadali his paper presens variaions o he Sccessive Backward Sweep (SBS mehod or solving nonlinear problems involving erminal and conrol consrains. he sggesed SBS mehod is based on he linear qadraic conrol mehodology and is relaively insensiive o he iniial gesses o he sae and conrol hisories. he overall procedre o his mehod is similar o he eising neighboring eremals or dierenial dynamic programming algorihms. Several mehods o Hessian modiicaion are ilized, when reqired, o enable he backward inegraion o he gain eqaions. However, his mehod does no reqire consisency o he saring saes and conrols wih respec o he sysem dynamics. For nmerical eamples are considered o demonsrae he perormance o SBS mehod and he resls are compared o heir respecive open-loop conerpars. INRODUCION Many opimal conrol mehods have been developed o solve rajecory opimizaion problems involving nonlinear sysems wih consrains. Some o hese mehods se he irs-order necessary condiions only and ohers are based on he applicaion o he second-order necessary condiions. he solions obained by he second-order mehods saisy he necessary and sicien condiions o opimaliy. Several ime-discreizaion approaches have also been proposed o conver opimal conrol problems ino opimizaion problems which are solved sing nonlinear programming approaches. In his paper, we ocs or aenion on he sweep mehods which involve, among oher eares, he solion o a Riccai dierenial eqaion. Jacobson developed he DDP (Dierenial Dynamic Programing mehod - in 968 o solve coninos and discree-ime opimal conrol problems or nonlinear sysems, inclding conrol consrains. Sbseqenly, DDP has been eensively developed, leading o he modiied DDP 3 mehod and HDDP 4 (Hybrid Dierenial Dynamic Programming. he DDP mehod shares many similariies wih he mehod o neighboring eremals 5- and is eensions o handle conrol consrains. Boh o hese approaches se a linearized sysem model and a qadraic approimaion o he perormance inde. he sbopimal SDRE (Sae Dependen Riccai Eqaion mehod 3-4 has ond many applicaions in he aerospace ield. his mehod, dependen on he acorizaion o a nonlinear * Gradae Sden, Deparmen o Aerospace Engineering, eas A&M Universiy Sewar & Sevenson I Proessor, Deparmen o Aerospace Engineering, eas A&M Universiy

2 sysem ino a non-niqe psedo-linear orm, has been applied o he solion o opimal conrol problems 5 by sing a sccessive approimaing seqence mehod. However, he solions obained by hese mehods do no necessarily saisy he sicien condiions or opimaliy and hey are no applicable o problems which reqire accrae saisacion o erminal and conrol consrains. he SDRE approach does provide a consrcive mehod o deermining an iniial conrol approimaion. he mehods discssed above have he common eares o solving a seqence o linearqadraic problems. he sccess o hese mehods depends on he consrcion o he qadraic approimaion o he perormance inde, i.e., he Hessian, or each sb-problem. A he beginning o he solion process, when he sae and conrol variables may be ar rom heir respecive opimal vales, a irs-order algorihm is recommended; he second-order erms accelerae convergence near he opimal solion. his paper eamines some o he sraegies or ormlaing he seqence o problems and eending he mehod o handle conrol and erminal consrains. In he irs par o his paper, he eares o he SBS (Sccessive Backward Sweep mehod are described. his mehod enables one o easily separae he s and nd order erms in he Hessian. Frhermore, any modiicaions o he Hessian are implemened in sch a way ha he original orm o he necessary condiions, accrae o second order, are recovered a he erminaion o he solion process. Mehods o handling singlariies in he calclaion o he erminal consrain Lagrange mliplier are discssed. In he las par o his paper, or nmerical eamples are considered o show he perormance o he SBS mehod. DESCRIPION OF HE SBS MEHOD he overall procedre o his mehod is similar o he eising neighboring eremals algorihms. However, in order o handle sensiive opimal conrol problems involving nsable open-loop sysems, he iniial gesses or he sae and conrol variables are assmed o be arbirary and are no consisen wih respec o he sysem dynamics. Coninos Nonlinear Opimal Conrol Generally, coninos-ime nonlinear opimal problems are ormlaed as 6 : Minimize: Sbjec o:,, (,, ( (3 where is he perormance inde, is he erminal penaly ncion,,, is he vecor o sysem dynamics, and is a inal sae consrain. I is assmed ha he inal ime is ied and he conrols are consrained o be wihin he bondary, m. he Hamilonian is deined as:,,,,,,, (4

3 where is a vecor o cosae. he irs-order necessary condiions are deermined rom Hamilonian as ollows: H (5 H q (6 arg min( H (7 ( (8 ( where he sbscrips represen parial derivaives, e.g., H is he gradien o H wih respec o. When he conrols obained rom Eq. (7 are sch ha he conrol consrain is inacive, hen he conrol is deermined rom H q (9 he SBS Mehod In he discssion ha ollows, i is assmed ha he erminal consrain is linear. o apply he SBS mehod, he nonlinear sysem has o be convered o he linearized sysem by sing he aylor series epansion. H H ( where ( HHH ( H H H H ( H H H (3 (4 H H H H (5 H H H H and,, indicae he sae, inp and cosae, respecively. For each conrol, wo possibiliies have o be considered, depending on he conrol consrain being inacive or acive: H ( H H ( conrol consrain inacive (6 m ( conrol consrain acive (7 Eqaions (6 and (7 can be combined ino a single epression as ollows: P c m H ( HH (8 3

4 where I P c (9 and I is an ideniy mari. he variables Pc and are inrodced o easily accon or wo modes: saraed and nsaraed inps. I he conrol magnide (componen wise, as calclaed rom Eq. (6, eceeds is limi, hen he corresponding elemen o P c I and zero oherwise. he backward sweep mehod assmes he orms S P V ( and [ ( ] P ( N W ( where is a Lagrange mliplier. Eqaion ( is sbsied o Eq. ( o obain he ollowing dierenial eqaions or he gains: S SH H S ( H SH H ( H H S H, S ( ( P H SH H H H P, P ( [( ] V [( H SH H H H ] V ( H SH H I (3 S ( H SH P V ( (4 c m W P H H H P, W ( (5 N P H P H H H V N ( (6 [ c m ( ] Deerminaion o he erminal consrain Lagrange mliplier is a consan vecor, and i can be comped a any ime oher han he inal ime. However, de o nmerical inaccracies, is vale will vary wih ime i evalaed by sing Eq. (. ypically, i is calclaed a he iniial ime as ollows: ( (7 W P N I has been observed in or nmerical eperimens ha i is beer o coninosly pdae sing any one o he ollowing mehods. Leas sqares mehod In his mehod a leas sqares solion o he ollowing eqaions is sogh a each ime insan: W P N a b( ; a b ; a (8 where a and b are parameers wih ideal vales, a and b. he above eqaions can be epressed in mari orm as 4

5 W ( ( P ( N ( a (9 b However, as epeced, he pdae o ( obained by sing leas sqares mehod canno proceed very near he inal ime. In he resls provided in his paper, he pdae is carried o parially over a domain, e.g., [.9 ] and he vale o (.9 is sed in he region[.9 ]. Aiming poin mehod For he case o linear erminal consrains, i is easy o deermine a connecion beween and he miss disance rom an aim poin. For a given ni penaly weigh, we have ( ( ( (3 where is an aiming vecor, no necessarily he same as he desired inal condiion. he aiming poin is nknown a priori. he deerminaion o cosae is similar o ha o he SBS mehod, b wih a modiied epansion compared o Eq. (: S P V (3 he sbsiion o Eq. (3 in Eq. ( resls in he ollowing gain eqaions, some o which are similar o hose obained previosly. S SH H S H SH H H H S H, S ( ( ( I (3 P [( H SH H H H] P, P ( I (33 V [( H SH H H H ] V ( H SH H S ( H SH P V ( (34 c m W P H H H P, W ( (35 N P H P H H H V N ( (36 [ c m ( ] Ecep or he bondary condiion on Eq. (33, Eqs. (-4 and Eqs. (3-34 are respecively very similar. Dierences beween Eqs. (5~(6 and (35~(36, respecively, are limied o changes in he signs o cerain epressions. he aiming vecor can be calclaed as: As he case or he compaion o, poin. W [ P ( N] (37 ms be comped over a domain eclding he end 3 Epansion or In his approach, is direcly epressed as a ncion o sae and cosae: 5

6 a b c (38 he ime derivaive o is a ab b c (39 he dierenial eqaions or he new se o gains are obained by sbsiing Eqs. (- ino Eq. (39: a ah bh ah bh H H, a ( ( b bh ah bh H H, b ( ( c ( ah bh P c m ( ah bh H a b, c ( Finally, can be calclaed by sing he Eq. ( and Eq. (38 I (4 I (4 (4 [ I bp] [( a bs bv c] (43 Inp Updae and Hessian Modiicaion In order o mainain he validiy o he LQ approimaion, he conrol pdae ms be limied in magnide. his is achieved by inrodcing a new coeicien a and he pdae rle, (44 * a ( a * where is he crren comped inp and is a inp. he coeicien a is deermined o minimize a cos ncion hrogh a line search. A sicien condiion or he eisence o a posiive deinie solion o he Riccai-like Eq. ( or (3 is ha H be non-negaive deinie along he rajecory. Dring he iniial convergence process, he nare o his mari proves imporan and a mehod o shape i is discssed below. In his paper, a special modiicaion o he perormance inde is proposed as ollows: [ ( ( J QF R ] d (45 a F ( a ( I (46 where a is a non-negaive coeicien relaed wih F, a posiive semi-deinie mari, evalaed on he reerence rajecory, having he propery F ( (47 Hence, he modiicaion aomaically vanishes pon convergence o he process. One o he goals o his paper is o develop a simple nonlinear opimal mehod ha is relaively insensiive o iniial gesses. For poor iniial solions, he high-order nonlineariy 6

7 resling rom oen cases convergence diiclies. hereore, his erm is negleced dring he early sages, as shown below: H q ac ( (48 H q ac ( (49 H q ac ( (5 H q ac ( (5 where ac is se o iniially and changed o aer a cerain nmber o ieraions. Evalaion o he Perormance Inde wo dieren orms o he perormance inde are inrodced o veriy he accracy o he solions. For a given perormance inde o he orm ( J Q R d (5 i is easy o veriy by diereniaion ha i he sae-cosae dierenial eqaions and he ransversaliy condiions are saisied, he cos-o-go ncion can be wrien as J( ( ( d (53 where is deined by Eq. (3. Noe ha or sysems linear in and,. One way o evalae he degree o sb-opimaliy is o deermine he dierence beween he wo indices. I he converged vales o he wo are he same, we recognize ha he obained solion is opimal wiho considering he open-loop solion. Nmerical eperimens show ha J is sensiive o nmerical inegraion error mch more so han is J. NUMERICAL EXAMPLES For nmerical eamples wih dieren characerisics are presened o demonsrae he perormance o he SBS mehod. he dierenial eqaions are propagaed by he ied-sep, 5 h order Rnge-Ka ormla, which is he 5 h order componen o he Rnge-Ka-Fehlberg (RKF algorihm 7. he reqired parial derivaives beween node poins are calclaed by direc diereniaion o he RK inegraion ormlae. A -D Nonlinear Problem wiho Sae and Conrol Consrains his eample was inrodced by Jacobson. he sysem dynamics and cos ncion are.anh(, ( 5.5 (54 J ( ( d 7

8 As shown by Jacobson or his eample, he eplici solion or he opimal conrol, in erms o he cosae, saisies he conveiy condiion H. hereore, we can deermine he opimal solion by sing he SBS mehod i he iniial rajecory is sch ha he sysem conrollable. Simlaion resls are presened or he ollowing condiions: able. Simlaion condiions or D nonlinear problem Variable Vale ime span (. Inp correcion ( a.5 he rajecory is deined by inconsisen choices or he sae and conrol: and. he simlaion resls are smmarized in able and Figs. ~4. able. he resls o simlaion or D nonlinear problem Mehod ( J J = SBS Mehod J =4.5953, J = SBS mehod.5 SBS mehod Figre. (Sae Figre. (Inp 8

9 5 SBS mehod J i J i 5 4 lam J ier # Figre 3. (Cosae Figre 4. J, J (Cos In Figs. ~4, he red lines indicae he resls o open-loop solion, he ble doed lines indicae he rajecory and he black doed lines indicae he resl o he SBS mehod. Figres ~4 clearly show ha he resls o open-loop solion and SBS mehod are well mached even hogh he iniial rajecory is qie arbirary. Figre 4 shows ha he vales o J and J dier signiicanly dring he iniial ieraions, b converge a he 4 h ieraion. his indicaes ha he obained solion by sing SBS mehod is indeed an opimal solion. A -D nonlinear problem wih inal sae consrain 5, ( [ ], ( J d ( [ ] (55 his problem involves a highly nonlinear sysem and, nless he iniial gess is ecellen, he solion process generally will reqire a modiicaion o he H erm. In his siaion, convergence can be achieved by sing he F mari, as discssed above. For his eample, is deermined by sing he leas sqares mehod. he condiions o simlaion are provided in able 3: able 3. Simlaion condiions or D nonlinear problem Variable Vale ime span (. Inp correcion ( he coeiciens o F ( a 5//5/ 9

10 he rajecory is deermined by sing he SDRE mehod. he resls o simlaion are as ollows. able 4. he resls o simlaion or D nonlinear problem Mehod ( ( J (5.499, J=8.79 SBS Mehod (5.43,9.45. J= N N OP OP SBS SBS - -4 N OP SBS Figre 5. (Sae Figre 6. (Inp 3 5 lamn lamn lamop lamop lamsbs lamsbs a =5 a = a =5 a = 5 lam J i ier # Figre 7. (Cosae Figre 8. J i (varying In Figs. 5~8, he red lines indicae he resls o he open-loop solion, he ble doed lines indicae he variables and he black doed lines indicae he resls o he SBS mehod. a

11 Figres 5~8 show ha he resls o he open-loop mach he respecive SBS solions eacly. Figre 8 shows he convergence hisory o J wih respec o he nmber o ieraions, as he parameer a is varied. I he vale o a is oo large, he convergence rae slows down, b is ecellen or smaller vales, becase F is akin o a sae weighing mari. Amospheric Reenry Problem Amospheric reenry is a well-known eample o a highly nonlinear problem. he sysem dynamics is 8-9 r V sin (56 V cos cos r cos (57 V cos sin (58 r V sin D r (59 cos V L cos cos rv r V (6 V cos cossin L sin (6 r cos V cos where r is he posiion o vehicle, is he longide, is he laide, V is he velociy o vehicle, is he ligh pah angle, is he heading angle, is a graviaional consan and is he conrol bank angle. he perormance inde is o minimize is he densiy o amosphere is deined as V 3 [ ( ] J L D d (6 k( r kr e e (63 where is he densiy a sea level, k is an amospheric scale heigh, r e is he radis o he Earh. he li and drag orces are represened as: L C SV (64 where C l is he li coeicien, l D C SV (65 d C d is he drag coeicien, and S is he ni reerence area.

12 he parameers sed in simlaion are presened in he able 5. Densiy( able 5. Simlaion parameers or he reenry problem Variable Amospheric scale heigh ( k Vale.7* slg / * / 6 3 Graviaional consan (.477* / s Li Coeicien ( C.35 l Drag Coeicien ( C.3 d Reerence Area ( S.375 Scaling Facor ( 6.538* deg he bondary condiions a iniial and inal poins are given in able 6. able 6. Bondary condiions or he reenry problem Variables Iniial vales Final vales ime ( (Sec 39(Sec Alide ( r 4,( - Longide( (rad.33(rad Laide( (rad -.5(rad Velociy(V 36,(/s,64(/s Fligh Pah Angle( -6.5(deg - Heading Angle( (deg - For his problem, he inp correcion coeicien a, a each ieraion, is deermined o minimize he cos hrogh a line search. he rajecory is obained by selecing a consan bank angle.8. his problem is challenging becase H is zero and hence, i has o be modiied or he applicaion o he SBS mehod. o begin, a perrbaion acor o is added o H and i is sbseqenly redced o e 7. he resls o simlaion are shown in able. 7.

13 r able 7. Simlaion resls or he reenry problem Variables Iniial vales Desired vales Final vales Alide ( r 4,( - -8,3( Longide( (rad.33(rad.399(rad Laide( (rad -.5(rad -.5(rad Velociy(V 36,(/s,64(/s,639.7(/s Fligh Pah Angle( -6.5(deg (deg Heading Angle( (deg (deg Cos ( J SBS mehod SBS mehod hea Figre 9. r (Sae Figre. (Sae -.5 SBS mehod 7 6 SBS mehod phi V Figre. (Sae Figre. V (Sae 3

14 ..5 SBS mehod -.5 SBS mehod gam -. psi Figre 3. (Sae Figre 4. (Sae 4 3 SBS mehod J # nm Figre 5. (Inp Figre 6. J i (Cos Figres 9~5 show ha he resls o open-loop solion and he SBS mehod mach very well. Figre 5 shows he convergence hisory or he perormance inde. he convergence rae is qie as iniially, b slows down aer approimaely ieraions. However, an ecellen solion is obained even i he ieraive process is sopped a he h ieraion. 4

15 Earh o Mars Orbi ranser Problem wih Conrol Consrains he ne eample considered is he Earh o Mars orbi ranser problem wih erminal consrains and bonded inp. he diicly wih he solion o his problem sems rom he bang-o-bang nare o he conrol and he lack o knowledge o he swiching srcre a priori. his eample is presened o compare he perormance o he SBS mehod implemened wih several opions or comping or. he objec o his problem is o minimize he inal mass wih a hrs-limied engine. he sysem dynamic eqaions 4 are epressed: V (66 y V y (67 z V z (68 V s 3 r m (69 y r y y s (7 3 V z r m m z z s (7 3 V m (7 gi where, yz, are he posiions o vehicle, V, Vy, V z are velociies o vehicle, s is a graviaional consan, I sp is he speciic implse, is hrs, which can be epressed as ollows: sp (73 y z When he hrs is saraed,, y, z are deined as: ma ma y ma z, y, z y z y z y z (74 he parameers sed in simlaion are presened in he able 8. able 8. Simlaion parameers or he orbi ranser problem Variable Vale Maimm hrs( ma.5n Speciic Implse ( I sp Iniial mass ( m ime o ligh( s kg days 5

16 he bondary condiions are represened in he able 9. able 9. Bondary condiions or he orbi ranser problem Variables Iniial vales Final vales -4,699,693(km -7,68,3(km y -5,64,48(km 76,959,469(km z 98(km 7,948,9(km V (km/s (km/s V y (km/s (km/s V 4 z * 4 (km/s 9.486* (km/s All calclaion se canonical nis reerred o he sn. In his problem, he maimm hrs inp is limied o.5n. wo dieren orms o conrol smoohing are sed o deal wih he disconinos hrs. In he irs approach, sed o deermine he open-loop solion, he inp is approimaed by sing he eponenial ncion ma (75 ep( SF / where SF indicaes a swiching ncion and is a coninaion parameer. For he applicaion o he SBS mehod, he problem ormlaion is modiied by deining he perormance inde as ollows: ( ( y z J m d (76 where is a coninaion parameer. he resls o simlaion or he orbi ranser problem are presened in he ollowing. able shows he resls obained by he varios combinaions o mehods and or a amily o coninaion parameers. ( able. Simlaion resls or he orbi ranser problem Mehod / m ( ( e SBS mehod ( (leas sqares mehod or e e e e e e

17 SBS aiming mehod ( (deerminaion o SBS mehod ( ( epansion e e e e e e e e e e eps= eps=e- eps=e- eps=e eps= eps=e- eps=e- eps=e Figre 7. ( Figre 8. (SBS-LS 7

18 .6.5 eps= eps=e- eps=e- eps=e eps= eps=e- eps=e- eps=e-3 eps=e Figre 9. (Aiming Figre. (SBS- epansion Figres 7~ show ha he coninaion process has he abiliy o converge oward he eac solion. Whereas he open-loop solion saisies he erminal consrains accraely, he SBS mehod does a good job o approimaing he solion, albei wih a sligh erminal error. Among hese hree variaions o he SBS mehods, he mehod sing he epansion provides he sharpes approimaion o he open-loop solion. he sharp swiching srcre o he eac solion canno be capred nless he inegraion sep-size is redced rher. CONCLUSIONS his paper presens variaions o he Sccessive Backward Sweep mehod o solve nonlinear opimal conrol problems wiho he need or accrae or consisen iniial gesses. Conrol consrains are handled by modiying he relaionship beween he conrols and he cosaes, obained rom he irs-order opimaliy condiion. wo mehods or deermining he erminal consrain Lagrange mlipliers are presened o improve he consrain saisacion accracy. In addiion, an aiming poin mehod is also discssed o handle erminal consrains. Frhermore, a novel scheme or modiying he sae weighing mari in he perormance inde, wih he propery ha he modiicaion aomaically vanish pon convergence o he process, is developed. he general mehod neglecs cerain second-order erms arising rom he Hamilonian dring he early sages o he solion process as a means or improving robsness o he ieraive process. For nmerical eamples are considered o demonsrae he perormance o SBS mehod. he resls indicae ha he proposed mehods reprodce he respecive open-loop solions even when he iniial gesses are poor and he conrol sysems are highly nonlinear. 8

19 REFERENCES Jacobson, D.H. New Second-Order and Firs-Order Algorihms or Deermining Opimal Conrol: A Dierenial Dynamic Programming Approach, Jornal o Opimizaion heory and Applicaions, Vol., 968. Jacobson,D.H., Dierenial Dynamic Programming Mehods or Solving Bang-Bang Conrol Problems, IEEE ransacions on Aomaic Conrol, Vol Ac-3, N.6, December Colombo,C., Opimal rajecory Design or Inercepion and Delecion o Near Earh Objecs, PhD hesis, Universiy o Glasgow,. 4 Lanoine,G., A Mehodology or Robs Opimizaion o Low-hrs rajecories in Mli-Body Environmens, PhD hesis, Georgia Insie o echnology, December. 5 Bryson, A.E., and Ho, Y.-C., Applied Opimal Conrol, Hemisphere, Washingon D.C., Merriam,C.W., Opimizaion heory and he Design o Feedback Conrols, McGraw Hill, New York, Mier,S.K., Sccessive Approimaion Mehod or he Solion o Opimal Conrol Problems, Aomaica, Vol. 3, 35( McReynolds.S.R and Bryson,A.E., A Sccessive Sweep Mehod or Opimal Programming Problems, Proc.6 h Join Ao. Conrol Con., roy, New York, 965, p Kelley,H.J., Kopp, R. E., and Moyer, H. G., A rajecory Opimizaion echniqe Based pon he heory o he Second Variaion, Grmman Aircra Engineering Co., 964. Bllock,.E and Franklin,G.F., A Second-Order Feedback Mehod or Opimal Conrol Compaions, IEEE rans, Ao. Conrol, AC-, 666 (967. Dyer,P. and McReynolds,S.R., he Compaion And heory O Opimal Conrol, 97, Academic Press New York. Fisher,M.E., Neighboring Eremals or Nonlinear Sysems wih Conrol Consrains, Dynamic and Conrol, 5, 5-4, Hll, R. A. and Cloier, J. R.,Mracek,C.P, Sansbery, D.., Sae-Dependen Riccai Eqaion Solion o he oy Nonlinear Opimal Conrol Problem, Proceeding o he American Conrol Conerence Philadelphia Pennsylvania, Jne Cloier, J. R., Sae-Dependen Riccai Eqaion echniqes an Overview, Proceeding o he American Conrol Conerence Albqerqe, New Meico, Jne Banks, S. P. and Dinesh, K., Approimae Opimal Conrol and Sabiliy o Nonlinear Finie and Ininie Dimensional Sysem, Annals o Operaions Research 98, 9-44,. 6 Lewis, F. L. and Syrmos, V. L., Opimal Conrol nd Ediion, 995, A Wiley-Inerscience Pblicaion. 7 Brden, R. L. and Faires, J. D., Nmerical Analysis, Sih Ediion, Brooks-Cole Pblishing Co., Williamson,W.E., Opimal hree Dimensional Reenry rajecories or Apollo ype Vehicles, PhD hesis, he Universiy o eas a Asin, Janary McCrae,C.M., High-Order Mehods or Deermining Opimal Conrols and Sensiiviies, Maser hesis, eas A&M Universiy, May. Berrand,R and Epenoy,R., New Smoohing echniqes or Solving Bang-Bang Opimal conrol Problems- Nmerical Resls and Saisical Inerpreaion, Opimal Conrol Applicaions and Mehods,, 3,