Recursive Construction of Optimal Smoothing Spline Surfaces with Constraints

Transcription

1 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, 11 Recurive Conrucion of Oimal Smoohing Sline Surface wih Conrain Hiroyuki Fujioka Hiroyuki Kano Dearmen of Syem Managemen, Fukuoka Iniue of Technology, Fukuoka 11-9, JAPAN fujioka@fi.ac.j). School of Science and Engineering, Tokyo Denki Univeriy, Saiama 3-39, JAPAN kano@mail.dendai.ac.j) Abrac: In hi aer, we conider he roblem of recurively conrucing moohing line urface wih equaliy and/or inequaliy conrain each ime when a new e of daa i oberved. The line are coniued by uing normalized uniform B-line a he bai funcion. Then variou ye of conrain are formulaed a linear funcion of he o-called conrol oin, and he roblem i reduced o quadraic rogramming roblem. Baed on he reul, we develo he recurive deign mehod for conrucing uch conrained moohing line urface. The erformance i examined by ome numerical examle. Keyword: Sline, Smoohing, Conrain, Recurive algorihm. 1. INTRODUCTION Conrucing curve and urface for a given e of dicree obervaional daa i one of key roblem in many field of engineering and cience uch a comuer aided deign, numerical analyi, image roceing, roboic, daa analyi, ec. In uch roblem, inerolaing and aroximaing mehod uing line funcion have been ued frequenly and udied exenively e.g. Boor [1]). In addiion o radiional aroximaing or inerolaing line, here are a large cla of roblem where we need o imoe variou conrain on line uch a monoone moohing line Egered [3]), inequaliy conrain a iolaed oin Marin [1]), ec. By emloying B-line aroach, he auhor have alo develoed a mehod for deigning moohing line wih conrain over inerval or a iolaed oin, and he conrucion of he line hen become a quadraic rogramming roblem Kano [7]). Some of he reul uing B-line aroach have been exended o he cae of urface Fujioka [9a,b]). Mo of he above deign mehod may face he roblem ha he ize of relevan marice and vecor become large a he number of given daa increae. Furhermore, heir mehod may be undeirable in uch a cae where ome e of daa are oberved one afer anoher and we would like o conruc line curve and urface each ime when a new e i given. Such a cae yically arie in ome roboic alicaion uch a SLAM Simulaneouly Localizaion and Maing), ec. Thu, he o-called recurive deign mehod have been udied for conrucing line ee e.g. Frezza [199], Karaalo [7], Piccolo [9]), buheyare only for linecurve. We have alo develoed imilar deign mehod for boh he cae of curve and urface by emloying B-line aroach Fujioka [, 9c]). In aricular, he deign mehod for he cae of curve ha recenly been exended o he cae of conrained line Fujioka [1]). Thi aer i a coninuaion of our udie on he oimal deign of moohing line emloying B-line aroach. In aricular, baed on our udie Fujioka [9b,c]), we here develo a recurive deign algorihm of oimal moohing line urface wih equaliy and/or inequaliy conrain. The line are conruced by emloying normalized uniform B-line a he bai funcion. We hen how ha he equaliy and/or inequaliy conrain can be yemaically added a linear funcion of he o-called conrol oin and ha he conrucion of he line urface become convex quadraic rogramming roblem. Baed on uch formulaion, a recurive deign algorihm for conrained moohing line urface i develoed. The algorihm enable u o ue in he broad range of roboic alicaion. We here aly he reul o he 3-dimenional conour rereenaion uing a mobile robo and he effecivene and uefulne are examined by numerical udie. For deigning urface x,), we emloy normalized, uniform B-line funcion B k ) of degree k a he bai funcion, x,) = i= k j= k τ i,j B k α i ))B k β j )) 1) on a domain D = [, m1 ] [, m ] R. Here, τ i,j are he weighing coefficien called conrol oin, α, β> ) are conan, m 1, m > ) are ineger, and i, j are equally aced kno oin wih i+1 i = 1 α, j+1 j = 1 β. ) Coyrigh by he Inernaional Federaion of Auomaic Conrol IFAC) 7

2 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, 11 We ummarize ome of he ymbol ha will be ued hroughou he aer: denoe he Lalacian oeraor, and he Kronecker roduc. Moreover, vec denoe he vec-funcion, i.e. for a marix A = [a 1 a a n ] R m n wih a i R m, vec A = [a T 1 a T a T n] T R mn ee e.g. Lancaer [19])).. OPTIMAL SPLINE SURFACES A reliminarie, we briefly review B-line and oimal moohing line urface..1 Normalized Uniform B-Sline Normalized uniform B-line B k ) of degree k i defined by { Nk j,k j) j < j + 1, j =,1,,k B k ) = 3) < or k + 1, and he bai elemen N j,k ) j =,1,,k), 1 are obained recurively by he following algorihm: Algorihm 1. Le N, ) 1 and, for i = 1,,,k, comue N,i ) = 1 N,i 1 ) i N j,i ) = i j + N j 1,i 1 ) j N j,i 1 ), i i ) j = 1,,i 1 N i,i ) = i N i 1,i 1). Thu, B k ) i a iece-wie olynomial of degree k wih ineger kno oin and i k 1 ime coninuouly differeniable. I i noed ha B k ) for k =,1,, i normalized in he ene of k j= N j,k) = 1, 1. Uing Algorihm 1, he bai elemen N j,k ) can readily be comued for arbirary degree k.. Oimal Smoohing Surface The conrol oin τ i,j in 1) may be deermined by he heory of moohing line a follow. Suoe ha we are given a e of daa {u i,v i ;d i ) : u i [, m1 ],v i [, m ], d i R,i = 1,,,N} ) and le τ R M1 M be he weigh marix defined by τ k, k τ k, k+1 τ k,m 1 τ k+1, k τ k+1, k+1 τ k+1,m 1 τ =... ) τ m1 1, k τ m1 1, k+1 τ m1 1,m 1 wih M 1 = m 1 + k and M = m + k. Then a andard roblem i o find uch a τ minimizing he co funcion Jτ) = λ I x,) ) N dd + w i xu i,v i ) d i ), i=1 7) where = [, m1 ], I = [, m ], λ> ) i a moohing arameer, and w i w i 1) are he weigh for aroximaion error. Leing ˆτ R M1M be a vec-funcion of τ defined a ˆτ = vec τ, ) he co funcion Jτ) in 7) can be rewrien a a quadraic funcion in erm of ˆτ ee e.g. Fujioka [] for deail), wih Jˆτ) = ˆτ T Gˆτ g T ˆτ + r, 9) G = λq + ΓWΓ T, g = ΓWd, r = d T Wd. 1) Here, Q R M1M M1M i a Gram marix defined by Q = b ) b 1 )) ) b ) b 1 )) ) T dd wih I b 1 ) = [B k α k )) B k α k+1 )) 11) B k α m1 1)] T, 1) b ) = [B k β k )) B k β k+1 )) B k β m 1))] T. 13) The marix Γ R M1M N in 9) i defined by Γ = [b v 1 ) b 1 u 1 ) b v N ) b 1 u N )]. 1) Alo, W R N N and d R N are given by W = diag{w 1, w,, w N } d = [d 1, d,, d N ] T. 1) I can be hown ha he marix G in 1) i oiiveemidefinie ee e.g. Fujioka []). Thu, he co Jˆτ) in 9) i a convex funcion in ˆτ. Hence, if we deign he moohing urface wihou imoing any conrain, he oimal oluion ˆτ minimizing he co funcion in 7) i given a a oluion of Gˆτ = g. 3. OPTIMAL SPLINE SURFACES WITH CONSTRAINTS There are variou ye of conrain uch a oinwie conrain on x,) and/or i derivaive, and conrain over inerval or domain in D, eiher equaliy or inequaliy. Uing B-line aroach, i can be hown ha uch conrain are formulaed a linear funcion of he conrol oin ee Fujioka [9b] for deail). A an examle, we here review only an inequaliy conrain over a domain, x,) f,),) [ κ, κ+1 ] [ µ, µ+1 ] 1) for a given coninuou funcion f,). Noe here ha he inequaliy may readily be relaced wih and equaliy = a we will ee in below. 79

3 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, Exreion of Conrain We fir reen he baic formula for exreing he conrain. Noing ha x,) i conruced a a roduc of wo iecewie olynomial, we examine he olynomial in each kno oin region D κ,µ = [ κ, κ+1 ) [ µ, µ+1 ) for κ =,1,,m 1 1 and µ =,1,,m 1. For D κ,µ, he funcion x,) in 1) i wrien a x,) = κ µ i= k+κ j= k+µ and, by 3), we ge x,) = i= j= τ i,j B k α i ))B k β j )), τ κ k+i,µ k+j 17) N i,k α κ ))N j,k β µ )). 1) Then, by inroducing new variable u and v defined by u = α κ ), v = β µ ), 19) he region D κ,µ i normalized o he uni region E = [,1) [,1) for u,v). Now, x,) i exreed in erm of u,v) a x,) = ˆxu,v) wih ˆxu,v) = τ κ k+i,µ k+j N i,k u)n j,k v). ) i= j= Uing he exreion in ) and he idea of limiing line urface ee e.g. Fujioka []), heconrain in 1) can be imoed a follow: For he given funcion f,), we fir comue he limiing line urface x c,) wih he ame form and he ame degree k a in 1), i.e. x c,) = i= k j= k τ c i,jb k α i ))B k β j )). 1) From our a work e.g. ee Fujioka []), we have emirically confirmed ha he urface x c,) can aroximae funcion f,) fairly reciely, i.e. x c,) f,). We hu regard he conrain x,) f,) in 1) a x,) x c,). Then, uch a conrain may be realized by imoing he condiion τ i,j τi,j c for i = κ k,κ k + 1,,κ and j = µ k,µ k + 1,,µ, or in erm of vecor ˆτ and ˆτ c = vec τ c ) a E κµˆτ E κµ τˆ c, ) where E κµ R k+1) M 1M i defined by E κµ = [ k+1,µ I k+1 k+1,m µ k 1] [ k+1,κ I k+1 k+1,m1 κ k 1] 3) In fac, if ) hold, we have from 1)-) and N i,k ) [,1], x,) = ˆxu,v) = i= j= i= j= τκ k+i,µ k+jn c i,k u)n j,k v) τ κ k+i,µ k+j N i,k u)n j,k v) = ˆx c u,v) = x c,),) [ κ, κ+1 ) [ µ, µ+1 ). ) The above argumen can be readily exended o he cae of broader region [ κ, ζ ) [ µ, η ) for arbirary ζ> κ) and η> µ). I i noed ha he condiion in ) i only ufficien for x,) x c,) o hold. A imle bu ueful cae of he funcion f,) in 1) i a conan, i.e. f,) = c, where c R i ome conan. Then, i can be hown ha uch conrain i exreedwihou emloying he idea of limiing line urface a E κµˆτ c k+1), where c i = [c c c] T R i. 3. Conrained Sline Surface Uing he exreion of conrain a in Secion 3.1, a fairly large number of conrained line urface roblem may be reaed. The formulaion i quie imle and i very well fi for numerical oluion a quadraic rogramming roblem. Namely, he oimal moohing line urface are obained by minimizing he quadraic co Jˆτ) in 9), wherea a number of conrain on he line may be exreed a linear conrain on he vecor ˆτ, eiher equaliy or inequaliy or boh. Then, a general form of roblem can be wrien a quadraic rogramming roblem a follow: QP1) Find ˆτ uch ha min Jˆτ) = 1 ˆτ R M 1 M ˆτT Gˆτ + g T ˆτ ) ubjec o he conrain of he form Aˆτ = q, f 1 Eˆτ f, h 1 ˆτ h, ) for ome marice and vecor of aroriae dimenion. A very efficien numerical algorihm i available for hi uroe e.g. Nocedal []).. RECURSIVE DESIGN ALGORITHM OF CONSTRAINED SMOOTHING SPLINE SURFACES Baed on he foregoing develomen, we develo a recurive algorihm of oimal moohing line urface wih conrain. Such an algorihm reven he ize of relevan marice and vecor from kee growing due o he increaing number of given daa. Now uoe ha, u o he -h recurion, we are given a e of N daa, {u i,v i ;d i ) : u i [, m1 ],v i [, m ], d i R,i = 1,,,N }, 7) where = 1,,. Here, we aume ha he number of daa given a he i-h recurion i n i 1), i =,1,, and hence N = i= n i. Then, leing x [],) be he oimal moohing lineurface a-h recurion, we coniderhe following recurive line roblem of conrucing x [],) by minimizing J [] τ) = λ x [],) x [ 1],) ) dd I

4 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, N x [] u i,v i ) d i ) ) N i=1 ubjec o ome conrain wih he form in ). Le τ [] R M 1 M be he oluion τ o hi roblem. Then, our ak i o develo an algorihm for recurively generaing he equence of τ [], i.e. τ [],τ [1],. Then, a equence of he aociaed oimal moohing urface, denoed a x [],),x [1],),, are generaed by 1). Such an algorihm i develoed a follow: We fir inroduce x [] and ˆτ [] defined by x [] = x [] x [ 1] 9) ˆτ [] = ˆτ [] ˆτ [ 1], 3) where ˆτ [] denoe he vec-funcion of τ [], i.e. ˆτ [] = vec τ []. Then, he co funcion J [] τ [] ) in ) i wrien a J [] ˆτ [] ), J [] ˆτ [] ) = ˆτ T [] G ˆτ [] + g T ˆτ [] + con. 31) Here, G R M1M M1M and g R M1M are defined a G = λq + 1 N Γ ΓT 3) g = 1 N Γ ΓT ˆτ [ 1] Γ d ), 33) where we e ˆτ [] a ˆτ [] = M1M. In 3) and 33), Γ R M1M N and d R N are defined a Γ = [ b v 1 ) b 1 u 1 ) b v N ) b 1 u N )] ] = [ Γ 1 ˆΓ ] 3) d = [d 1 d d N ] T = [ d T 1 ˆd T ] T, 3) where ˆΓ R M1M n and ˆd R n denoe ˆΓ = [ b v N 1+1) b 1 u N 1+1) b v N ) b 1 u N )] ] 3) ˆd = [d N 1+1 d N 1+ d N ] T. 37) By he exreion of Γ in 3) and d in 3), G in 3) and g in 33) are rewrien a 1 G = G ) Γ 1 ΓT N N ˆΓˆΓT 1 N 3) g = 1 Γ ΓT N ˆτ [ 1] Γ 1 d 1 ˆΓ ) ˆd. 39) The exreion in 3) and 39) yield efficien recurive mehod for comuing G and g. On he oher hand, by uing 3), he conrain ) on he line urface x [],) can be readily exreed a linear conrain in erm of ˆτ []. Then, he general form of hi roblem i idenical o he quadraic rogramming roblem QP1) in Secion 3.: QP) Find ˆτ [] uch ha min ˆτ [] R M 1 M J [] ˆτ [] ) = 1 ˆτT [] G ˆτ [] + g T ˆτ [] ) ubjec o he conrain of he form A ˆτ [] = q, f1 E ˆτ [] f, h 1 ˆτ [] h, 1) where q, fl, h l for l = 1, are comued by q = q Aˆτ [ 1], fl = f l Eˆτ [ 1], h l = h l ˆτ [ 1]. ) Thu, by olving hi roblem wih reec o ˆτ [], we ge he conrol oin vecor ˆτ [] for he -h recurion by 3) a ˆτ [] = ˆτ [ 1] + ˆτ []. 3) The recurive deign algorihm of conrained moohing line urface can be ummarized a follow. Algorihm. The recurive algorihm, afer iniializaion e I-1)-I-), i carried ou in he e R-1)-R-). Iniializaion e: I-1) Le =, and e he arameer k, α, β, λ,,, m 1 or M 1 = m 1 + k)) and m or M = m + k)). I-) Comue Q R M1M M1M in 11), Γ R M1M N in 3) and d R N in 3). I-3) Comue G R M M and g R M by 3) and 33). I-) Se he conrain in ) a required. I-) Find ˆτ [] by olving QP1). I-) Conruc x [],) by 1). Recurive e: R-1) Se = + 1, and comue ˆΓ R M1M n in 3) and ˆd R n in 37). R-) Comue G by 3). R-3) Se Γ ΓT = Γ 1 ΓT 1 + ˆΓ ˆΓT. R-) Comue g by 39). R-) Se Γ d = Γ 1 d 1 + ˆΓ ˆd. R-) Comue q, fl, hl, l = 1, by ) and e he conrain in 1) a required. R-7) Find ˆτ [] by olving QP) and comue ˆτ [] by 3). R-) Conruc he line curve x [],) by 1). Go o R- 1). Comared wih he ordinary deign mehod in Secion 3, he rooed mehod mu olve he quadraic rogramming roblem a each recurion in Se R-7). Thu, he comuaional comlexiy of rooed mehod may be modely increaed. However, we can reven he ize of relevan marice and vecor from kee growing a he oal number of daa N geing larger. I remain o rove he convergence roerie of he above mehod. However, when we aume ha he daa d i for conrucing moohing curve are obained by amling ome curve f,), we may exec ha he oimal oluion of hi roblem converge o he one of minimizing J [] τ) = λ I x [],) x [ 1],) ) dd 1

5 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, 11 + I x [],) f,)) dd ) ubjec o ome conrain of he form in 1). Alo, he oimal oluion τ R M1 M of he roblem, denoed by τ[] c, can be comued recurively by he imilar algorihm a Algorihm. The aociaed line x c [],) are hen obained.. NUMERICAL EXAMPLES A yical alicaion of oimal moohing line urface wih conrain aear in he roblem of conrucing and reconrucing 3-dimenional conour in he roboic field. For examle, when a e of daa are meaured recurively byome range enorand we needo conruc and reconruc conour rereenaion of an environmen by uing ome coninuou eriodic urface, he mehod in he reviou ecion can be ued effecively. We here examine erformance of recurive deign algorihm in he reviou ecion by he following numerical udie. Le u conider a cae where a mobile robo wih ome range enor i in a cloed environmen in 3-dimenional ace o q a hown in Figure 1. Here, he mobile robo i illuraed a green cylinder. The blue urface how he cloed environmen. We here aume ha he cloed environmen i given a a eriodic urface, ) rc + r m,) = r c + r m ) co θ) r m co θ) r m ) rc + r m q,) = r c + r m ) inθ) r m in θ) r m,) [,1] [,1] ) wih r c = 1, r m = 1/ and θ) = 3π 1. Alo, leing f,) be he diance beween robo and environmen, f,) i wrien a f,) =,) + q,),,) [,1] [,1]. ) Now uoe ha he daa d i in 7) meaured by a range enor i obained by amling f,). Here he number of daa i e a n i = 3 a each recurion. u i,v i ) are randomly aced in he region D = [, m1 ] [, m ] = [,1] [,1], and he magniude of he addiive noie in d i i e a σ =.1. However, noe ha uch a meaured daa may be unreliable unle hey are wihin Fig. 1. Conrucing conour by a mobile robo wih range enor. he meaurable range of range enor. In Figure 1, uch a meaurable range i loed a a ink circle wih radiu r a = 13. For conrucing he reliable conour in o q ace, we conider conrucing he moohing line urface x [],) in he ime domain D recurively uch ha x,) r a = 13),,) [,1] [,1]. 7) We e k = 3, λ = 1, α = β = 1, = = and m 1 = m = 1 i.e. m1 = m = 1). Thu he kno oin i, j are aken a ineger a i = i, j = j. The eriodiciy conrain are e a l l x,) = l l x m1,), [, m ] ) for l =,1,. Noe ha he inequaliy conrain in 7) i imoed by emloying he mehod in Secion 3.1. In addiion, ) i conrain over inerval and he mehod in he aer Fujioka [9a]) can be ued. By Algorihm, oimal weigh τ [] for =,1, are comued ogeher wih he aociaed line urface x [],). Figure how he reul x [],) for a) =, b) = and c) = 3 in colored urface ogeher wih daa oin green quare). Alo, he correonding 3- dimenional conour loed in o q ace are hown in Figure 3, where we here emloy he following coordinae,),q,)) = x,)co θ), x,)inθ)). 9) From hee figure, we may ee ha he develoed mehod work quie well and he urface x [],) aifie all he conrain in 7) and ). Alo, Figure how he funcion error x c [3],) x [],) L and error of weigh marix τ[3] c τ []. Here x c [3],) i conruced from f,) by ), where he correonding conrol oin marix i denoed a τ[3] c. From hee figure, we may oberve ha x [],) converge o x c [],) a he ieraion number increae.. CONCLUDING REMARKS We develoed a yemaic mehod for recurive deign ofoimal moohinglineurface wih equaliy and/or inequaliy conrain. The line urface are coniued emloying normalized uniform B-line a he bai funcion. Then he cenral iue wa o deermine an oimal marix of he o-called conrol oin. Such an aroach enable u o exre variou ye of conrain a linear funcion of conrol oin. The deign roblem become a quadraic rogramming roblem in erm of vec-funcion of conrol oin marix, and very efficien recurive algorihm wa develoed. We examined he erformance ofhedeignmehodbynumericalexamle for 3-dimenional conour conrucing roblem wih equaliy andinequaliy conrain. Ii concludedhahemehod i very effecive a well a very ueful for many alicaion in variou field including roboic. REFERENCES C. de Boor, A racical guide o line, Revied Ediion, Sringer-Verlag, New York, 1.

6 Prerin of he 1h IFAC World Congre Milano Ialy) Augu - Seember, 11 daa oin daa oin daa oin x,) 1 x,) 1 x,) a) x[], ) b) x[], ) c) x[3], ) b) x[], ) c) x[3], ) Fig.. oimal moohing line. a) x[], ) Fig. 3. Periodic urface x, ) rereened in he o q ace. 1 norm of τc[3] τ[] 3 [] 1 [3] m1mxc,) x,))dd 1 3 a) 1 xc[3], ) 1 3 x[], ) L 1 b) 1 1 c τ[3] 3 τ[] Fig.. Funcion error xc[3], ) x[], ) L and weigh c marix error τ[3] τ[]. M. Egered and C. F. Marin, Oimal conrol and monoone moohing line. New rend in nonlinear dynamic and conrol, and heir alicaion, 79 9, Lecure Noe in Conrol and Inform. Sci., 9, Sringer, Berlin, 3. H. Fujioka, H. Kano, M. Egered and C. Marin, Smoohing Sline Curve and Surface for Samled Daa, In. J. of Innovaive Comuing, Informaion and Conrol, vol.1, no.3,.9-9,. H. Fujioka and H. Kano, Recurive Conrucion of Oimal Smoohing Sline, Proc. of he h ISCIE In. Sym. on Sochaic Syem Theory and I Alicaion,.7 7, Kyoo, Jaan, Nov. 1-1,. H. Fujioka and H. Kano, Periodic Smoohing Sline Surface and I Alicaion o Dynamic Conour Modeling of We Maerial Objec, IEEE Tran. Syem, Man and Cyberneic, Par A, vol.39, no.1,.1-1, 9. H. Fujioka and H. Kano, Conrained Smoohing and Inerolaing Sline Surface uing Normalized Uniform B-line, Proc. he IEEE In. Conf. on Indurial Technology,.1-, Giland, Vicoria, Auralia, Feb. 1-13, 9. H. Fujioka and H. Kano, Recurive Conrucion of Smoohing Sline Surface Uing Normalized Uniform B-line, Proc. of he h IEEE Conf. on Deciion and Conrol held joinly wih 9 h Chinee Conrol Conf.,.- Shanghai, China, Dec. 1-1, 9. H. Fujioka and H. Kano, Recurive Conrucion of Oimal Smoohing Sline wih Conrain, Proc. of he 1 American Conrol Conf.,.1-1, Balimore, MD, USA, June 3-July, 1. R. Frezza and G. Picci, On Line Pah Following by Recurive Sline Udaing, Proc. of he IEEE Conf. on Deciion and Conrol,.7-, New Orlean, LA, USA, Dec.13-1, 199. H. Kano, H. Nakaa, and C. F. Marin, Oimal Curve Fiing and Smoohing Uing Normalized Uniform BSline : A ool for udying comlex yem, Alied Mahemaic and Comuaion, vol.19, no.1,.9-1,. M. Karaalo, X. Hu and C.F. Marin, Conour Reconrucion and Maching Uing Recurive Smoohing Sline, Lecure Noe in Conrol and Informaion Science, vol.3,.193, 7. H. Kano, H. Fujioka, and C. F. Marin, Oimal Smoohing and Inerolaing Sline wih Conrain, Proc. of he h IEEE Conf. on Deciion and Conrol,.31131, New Orlean, LA, USA, Dec. 1-1, 7. P. Lancaer and M. Timeneky, The Theory of Marice, Second Ediion, Academic Pre, 19. C.F. Marin, S. Sun and M. Egered, Oimal conrol, aiic and ah lanning. Comuaion and conrol, VI Bozeman, MT, 199). Mah. Comu. Modelling, 33, no.1-3,.37 3, 1. J. Nocedal and S.J. Wrigh, Numerical Oimizaion, nd Ediion, Sringer,. G. Piccolo, M. Karaalo, D. Kragic and X. Hu, Conour Reconrucion uing Recurive Smoohing Sline - Algorihm and Exerimenal Validaion, Roboic and Auonomou Syem, vol.7, iue -7,.17-, 9. 3