Optimal Control, Statistics and Path Planning

Transcription

1 PERGAMON Mathematical and Compter Modelling 33 (21) Optimal Control, Statistics and Path Planning C. F. Martin and Shan Sn Department of Mathematics and Statistics Texas Tech University, Lbbock, TX 7949, U.S.A. M. Egerstedt Optimization and Systems Theory Royal Institte of Technology SE-1 44 Stockholm, Sweden Abstract In this paper, some of the relationships between optimal control and statistics are examined. In a series of earlier papers, we examined the relationship between optimal control and conventional splines and between optimal control and the statistical theory of smoothing splines. In this paper, we present a nified treatment of these two problems and extend the same framework to inclde the concept of dynamic time warping, which is being seen as an important statistical tool as well as being of importance in physics. We show that these three major problems nite to give a satisfactory soltion to the problem of trajectory or path planning. c 21 Elsevier Science Ltd. All rights reserved. Keywords Optimal control, Data interpolation, Crve registration, Path planning. 1. INTRODUCTION In this paper, we consider a series of eight problems that have their origins in optimal control theory, statistics, and nmerical analysis. In the papers [1,2], we have exploited the fact that splines and linear optimal control theory as well as controllability are very closely related to the classical nmerical theory of splines. In the papers [3,4], we have exploited the fact that optimal control theory and the statistical theory of smoothing splines are closely related. Or attention was drawn to the paper [5] and the book [6], from which we noted that the basic idea of crve registration or dynamic time warping were again very closely related to the problem of optimal control theory. In this paper, we give a nified treatment to the eight problems. This is not to say that we have effectively solved all eight problems, for some of the problems are of the natre that the main goal will be to develop effective nmerical algorithms. The main contribtion of this paper is to show that all eight of the problems are very similar in natre if not in soltion. In Problem 1, we show that the theory of interpolating splines is natrally considered as a problem of minimizing a qadratic cost fnctional sbject to a set of linear constraints. For polynomial splines, that was the original concept in their development. For Problem 3, we show *Spported in part by NSF Grants ECS and and NASA Grant NAG2-92. The spport of KTH dring the spring of 1998 is grateflly acknowledged. The spport of the Swedish Fondation for Strategic Research throgh its Centre for Atonomos Systems is grateflly acknowledged /1/$ - see front matter c 21 Elsevier Science Ltd. All rights reserved. Typeset by AMS-TEX PII: S ()241-7

2 238 C. F. Martin et al. that the theory of smoothing splines can be considered as being very close to the theory of interpolating splines, with the difference being that the linear constraints are inclded in the cost fnctional as a penalty term. The optimization problem is a straightforward problem of minimizing a qadratic cost fnctional over the space of sqare integrable fnctions. In Problems 2 and 4, we show that the problem of constrcting splines that pass throgh intervals instead of points can be redced to the problem of minimizing a qadratic cost fnctional sbject to a set of ineqality constraints. This problem is then redced to the problem of solving a qadratic programming problem. In Problem 5, we show that the problem of constrcting splines that are nondecreasing at the nodes is again redcible to the problem of minimizing a qadratic cost fnctional with ineqality constraints, and hence, redcible to a qadratic programming problem. In Problem 6, we simply restate the crve registration problem of Li and Ramsay [5] as an optimal otpt tracking problem. Here, the problems become noticeably harder. The cost fnctional is now nonlinear as well as the differential constraint. In Problem 7, we formally state the otpt tracking problem and show that Problem 6 is a special case. Finally, in Problem 8 we state a version of the path planning problem. The statement of the problem involves the previos seven problems. We do not explicitly solve the problem, bt we do present an algorithm that will at least prodce a sboptimal problem. We sggest, based on the soltions of the first five problems, that it sffices to redce the problem to a nonlinear optimization problem over a finite parameter space. This gives some spport to ideas in [5,6] that one can se splines to solve the problem. We se a slightly different psedo time fnction than was sed in the two references, bt it is clear that or formlation owes its existence to the development of Li, Ramsay, and Silverman. 2. NOTATION In this section, we establish some notation concerning the control system and the data sets which we will consider. We will not consider the most complicated cases in this paper bt will restrict orselves to a fairly simple sitation. We will only consider the nivariate case in this paper. We will assme as given throghot this paper a linear dynamical system of the form ẋ = Ax + b, (1) y = cx, (2) where x R n,, and y are scalar fnctions and A, b, and c are constant matrices of compatible dimension. We will frther assme that cb = cab = ca 2 b = = ca n 2 b =. (3) This condition can be relaxed, bt the exposition is simplified with this assmption. For the most part, we will be considering systems for which A =.,.. 1 α 1 α 2 α 3 α n b =(,,...,, 1), and c =(1,,,...,). Any system which satisfies the zero constraints of (3) is eqivalent to a system with this form. The case in which all of the α i s are zero plays a particlarly important role, for then the soltions to the differential eqation are simply polynomials convolved with the control fnction. Under this assmption, all of what follows in this paper redces to the case of polynomial splines.

3 Path Planning 239 The data sets we consider in this paper are of two basic types deterministic and random. For the trajectory planning problem, we sally consider the data to be given in a deterministic form; that is, the coordinates of the locations and times are given exactly. We denote by DD = {(α i,t i ):t 1 <t 2 < <t N }, (4) where the DD stands for deterministic data and we denote by SD = {(f (t i )+ɛ i,t i ):t 1 <t 2 < <t N }, (5) where the ɛ i are observed vales of a random variable which in generally we assme is symmetric with mean. The term SD is to be interpreted as stochastic data. The data set SD is the set sally encontered in statistics. Solving the differential eqation, we have y(t) =ce At x + t ce A(t s) b(s) ds. (6) It is convenient to set x = since the initial data can be absorbed into the data. We now define a one parameter family of fnctions which are basic to the rest of the paper. Let { ce A(t s) b, t > s, g t (s) = (7), otherwise, where we consider s to be the independent variable and t a parameter. We now define a linear fnctional in terms of g t (s) as L t () = g t (s)(s) ds. (8) With this notation, we have what will be a fndamental relationship for this paper y(t) =L t (). (9) We will se the following fnctional in Section 6. Define D k d k g t L() = (s)(s) ds, (1) dtk and note that the derivative is well defined provided that k<n 2. With this notation, we have d k dt k y(t) =Dk L t (). (11) 3. INTERPOLATING SPLINES In this section, we consider the problem of constrcting a control law (t) that drives the otpt fnction y(t) throgh a set of data points at prescribed times. We will constrct so that the reslting otpt crve is piecewise smooth and generalizes the classical concept of polynomial spline. We consider the data set DD. The interpolating conditions can then be expressed as y (t i )=L ti (), i =1,...,N. (12) There are, of corse, infinitely many control laws that will satisfy these constraints. The problem is to have a scheme that will select a niqe control law and will choose one that has some physical meaning. Linear qadratic optimal control provides a convenient tool for this selection and the main object of this paper is to show that optimal control plays a natral role in this problem and the other problems of this paper. For the prposes of this paper, we choose the very simple cost fnctional J() = 2 (s) ds. (13) It is possible to increase the complexity of the cost fnctional, and this has been done qite sccessflly in references [7,8]. We mst specify from what set the control is to be chosen. The first problem then becomes as follows.

4 24 C. F. Martin et al. Problem 1. sbject to the N constraints with min J(), y (t i )=L ti (), L 2 [,T]. The problem is solved sing techniqes elcidated in [9]. We constrct the orthogonal complement to the linear sbspace defined by span {(s) :L ti () =}. It is a rather trivial exercise to determine that this set is the same as the set spanned by the fnctions g ti (s). Ths, the optimal control is of the form (s) = α i g ti (s). (14) Sbstitting this into the eqations defining the affine variety, we have then a set of eqations y (t 1 )=α 1 L t1 (g t1 )+ + α N L t1 (g tn ), y (t 2 )=α 1 L t2 (g t1 )+ + α N L t2 (g tn ),. y (t N )=α 1 L tn (g t1 )+ + α N L tn (g tn ). We can write this in matrix form as ŷ = Gˆα, and it shold be noted that the matrix G is a Grammian, and hence has the potential to be very poorly conditioned. The advantage is that we can immediately see that there is a niqe soltion since the g ti (s)s are linearly independent. The conditioning can be greatly improved by replacing the fnctions g ti (s) with a set of fnctions that are nonzero only on intervals of the form [t i,t i+n ]. This procedre is otlined in [3]. The advantage is that it redces the matrix G to a banded matrix (tridiagonal in the case that n = 2) which somewhat simplifies the soltion. It is not clear how the overall stability of the nmerical procedre is affected by this transformation. It shold be noted that althogh the spline techniqes are often said to be nonparametric, in fact, they are eqivalent to a finite dimensional parametric problem. Also note that in the case that the matrix A is nilpotent, i.e., that the parameters are zero, the above constrction is jst the ordinary polynomial spline. There are several ways to constrct splines to solve the basic problem. A totally different constrction that is mch better conditioned is developed in [1]. That constrction develops the banded strctre directly bt has the disadvantage of not carrying over to the more general problems of this paper. 4. INTERPOLATING SPLINES WITH CONSTRAINTS In this section, we consider a somewhat different problem that has considerable application and can be solved in mch the same manner as the classical interpolating spline. The problem we consider is when instead of data points throgh which the system mst be driven, we reqire that the system be driven throgh intervals. We state the problem formally.

5 Path Planning 241 Problem 2. sbject to the constraints and a i L ti () b i, min J(), L 2 [,T]. i =1,...,N In the srvey by Wegman and Wright [1], this type of spline is discssed. In this section, we show that this type of spline can be recovered with standard optimal control techniqes similar to what we sed in the last section taken together with mathematical programming. We first note that becase of linearity, the set of controls that satisfy the constraints is closed and convex. Lemma 4.1. The set of controls that satisfy the constraints of Problem 2 is a closed and convex sbset of L 2 [,T]. Proof. The proof is elementary and is probably fond in any nmber of elementary textbooks. Sppose that for some finite nmber M we have controls k (s) which satisfy the constraints of Problem 2. Then, for each i, we have a i = α k a i α k L ti ( k ) α k b i = b i, k=1 k=1 k=1 where and α k =1 k=1 α k >. Now consider ( N ) α k L ti ( k )=L ti α k k. k=1 Ths, the convex sm of controls satisfies the constraints if the individal controls satisfy the constraints. On the other hand, assme that { i } is a seqence of controls that each satisfy the constraints. Passing the limit throgh the integral, becase of compactness of the interval [,T], it follows that the limit also satisfies the constraints. The lemma follows. The existence and niqeness of the optimal control follows from standard theorems; see for example [9]. Theorem 4.1. There exists a niqe control fnction (t) that satisfies the constraints and minimizes J(). Let â =(a 1,a 2,...,a m ) k=1 and ˆb =(b1,b 2,...,b m ). We form the associated optimal control problem max λ,γ min H(, λ, γ), (15)

6 242 C. F. Martin et al. sbject to the constraint where and H(, λ, γ) = 1 2 λ, γ, 2 (t) dt + λ i (a i L ti ()) + γ i (L ti () b i ), (16) γ =(γ 1,γ 2,...,γ N ), and λ =(λ 1,λ 2,...,λ N ). We minimize the fnction H over assming that λ and γ are fixed. The minimm is achieved at the point where the Gateax derivative of H, with respect to, is zero. This is fond by calclating ( ) 1 lim (H( + τv,λ,γ) H(, λ, γ)) = (t) λ i g ti (t)+ γ i g ti (t) v(t) dt. τ τ Setting this eqal to zero and solving, we have the condition that the optimal is given by (t) = λ i g ti (t) γ i g ti (t) =λ g γ g, (17) where g =(g 1 (t),g 2 (t),...,g N (t)). We now eliminate from the Hamiltonian by sbstitting to obtain H (,λ,γ)= 1 (( λ γ ) g ) 2 N dt + λ i (a i L ti ( )) γ i (L ti ( ) b i ) 2 = 1 2 (λ γ) G(λ γ)+λ a γ b λ i L ti ( )+ γ i L ti ( ) = 1 2 (λ γ) G(λ γ)+λ a γ b + j=1 j=1 j=1 ( ) N ( ) λ i γ j L ti gtj γ j γ i L ti gtj ( ) N ( ) λ i λ j L ti gtj + λ j γ i L ti gtj j=1 = 1 2 (λ γ) G(λ γ)+λ a γ b λ Gλ + λ Gγ + λgγ γ Gγ = 1 2 (λ γ) G(λ γ)+λ a γ b (λ γ) G(λ γ) = 1 2 (λ γ) G(λ γ)+λ a γ b. We can write H(,λ,γ) in a form sitable for se in qadratic programming packages in the following manner: H (,λ,γ)= 1 ( )( ) ( ) G G λ 2 ( λ γ) +(λ γ) a. (18) G G γ b Ths, to find the optimal, we need only solve the qadratic programming problem max H λ,γ (,λ,γ), (19) sbject to positivity constraints on λ and γ.

7 Path Planning 243 In general, the control laws constrcted by this techniqe are going to drive the control close to the end points of the interval. A better control law can be obtained by penalizing the control for deviating from the center of the interval. The derivation is essentially the same in concept, bt it is a bit more complex in calclation. We leave the calclation to Section SMOOTHING SPLINES In many problems, to insist that the control drives the otpt throgh the points of the data set is overly restrictive and in fact leads to control laws that prodce wild excrsions of the otpt between the data points. This phenomena was observed by Wahba, and she developed a theory of smoothing splines that at least partial corrected this problem. In this section, we will develop a theory of smoothing splines based on the same optimal control techniqes sed to prodce interpolating splines. We will penalize the control for missing the data point instead of imposing hard constraints. We formlate this as Problem 3. Problem 3. Let The problem is simply for J() = w i (L ti () α i ) 2 + ρ min J() L 2 [,T]. (t) 2 dt. The constants w i are assmed to be strictly positive as is ρ. The choice of the parameters w i and ρ are important. They control the rate of convergence of the optimal control as the nmber of data points goes to infinity. This is discssed in detail in [4]. The choice of ρ for fixed data sets is an important isse and is discssed at length in the monograph of Wahba [11]. We calclate the Gateax derivative of J in the form 1 lim (J( + αh) J()) (2) α α = 2w i L ti (h)(l ti ()+α i )+2ρ h(t)(t) dt (21) [ N ] =2 w i g ti (t)(l ti ()+α i )+ρ(t) h(t) dt. (22) Now to ensre that is a minimm, we mst have that the Gateax derivative vanishes bt this can only happen if w i g ti (t)(l ti ()+α i )+ρ(t) =. (23) We now consider the operator T () = w i g ti (t)l ti ()+ρ(t). (24) We can rewrite this operator in the following form: ( N ) T () = w i g ti (t)g ti (s) (s) ds + ρ(t). (25) Or goal is to show that the operator T is one to one and onto which we now do.

8 244 C. F. Martin et al. Lemma 5.1. The operator T is one to one for all choices of w i > and ρ>. Proof. Sppose T ( ) =. Eqation (24) directly gives that and hence that w i g ti (t)l ti ( )+ρ (t) =, w i g ti (t)a i + ρ (t) =, where a i is the constant L ti ( ). This implies that any soltion of T ( ) = is in the span of the set {g ti (t) :i =1,...,N}. Now consider a soltion of the form (t) = j=1 τ i g ti (t) and evalate T ( ) to obtain w i g ti (t)l ti τ j g tj (t) + ρ τ i g ti (t) =. Ths, for each i, N w i j=1 L ti ( gtj ) τj + ρτ i =. The coefficient τ is then the soltion of a set of linear eqations of the form (DG + ρi)τ =, where D is the diagonal matrix of the weights w i and G is the Grammian with g ij = L ti (g tj ). Now consider the matrix DG + ρi and mltiply on the left by D 1 and consider the scalar x ( G + ρd 1) x = x Gx + ρx t D 1 x>, since both terms are positive. Ths, for positive weights and positive ρ, the only soltion is τ =. It remains to show that the operator T is onto. Lemma 5.2. For ρ> and w i >, the operator T is onto. Proof. Sppose T is not onto. Then there exists a nonzero fnction f sch that f(t)t ()(t) dt =, for all. We have after some maniplation [ ] f(t)t ()(t) dt = w i g ti (t)g ti (s)f(t) dt + ρf(s) (s) ds and hence that =, i= w i g ti (t)g ti (s)f(t) dt + ρf(s) =. i= By the previos lemma, the only soltion of this eqation is f =, and hence T is onto. We have proved the following proposition.

9 Path Planning 245 Proposition 5.1. The fnctional J() = w i (L ti ()+α i ) 2 + ρ 2 (t) dt has a niqe minimm. We now se (23) to find the optimal soltion. As in the proof that T is one to one, we look for a soltion of the form (t) = τ i g ti (t). Sbstitting this into (23), we have pon eqating coefficients of the g ti (t), the system of linear eqations (DG + ρi)τ = Dγ. (26) As in the proof of the lemma, the coefficient matrix is invertible, and hence the soltion exists and is niqe. The reslting crve y(t) is a spline. The major difference is that the nodal points are determined by the optimization instead of being predetermined. Inverting the matrix DG + ρi is not trivial. Since it is a Grammian, we can expect it to be badly conditioned. However, by sing the techniqes in [3], the conditioning can be improved. It is still not clear what happens to the overall nmerical stability of the problem. 6. SMOOTHING SPLINES WITH CONSTRAINTS In this section, we consider two different problems. The first problem which we will consider is a rather straight forward extension of Problem 2. The derivation thogh is significantly more complex. This is stated as Problem 4. The reslting spline is of practical importance. The second class of problems which we will consider in this paper is very important. It is often the case that there is something known abot the nderlying crve, i.e., in SD we may have some prior knowledge abot the fnction f. For example, if the data represent growth data on a child from age three months to seven years, we can be reasonably assred that the fnction f is monotone increasing and the reslting spline mst also be monotone increasing if the crve is to have any credibility. There are also cases in which the nderling crve is convex or perhaps even more information is known so that the crve mst have other shape parameters. In this section, we will show that the techniqes we have developed for optimal control can be sed to formlate and solve a version of these problems, althogh we do not yet have a sitable constrction for monotone splines. We begin by formlating the following problem. Problem 4. Let the cost fnctional be defined by J() = (t) dt (L ti () ρ i ) 2, where The problem is then to sbject to the constraints of Problem 2. ρ i = a i + b i. 2 min J(),

10 246 C. F. Martin et al. Define the Hamiltonian by H(, λ, γ) = (t) dt (L ti () ρ i ) 2 + λ i (a i L ti ()) + γ i (L ti () b i ). (27) As before, we want to minimize H with respect to and maximize with respect to λ and γ. Calclating the Gateax derivative of H with respect to, we find = [ + 1 lim (H( + αv, λ, γ) H(, λ, γ)) α α ] g ti (L ti () ρ i ) λ i g ti + γ i g ti v(t) dt. (28) Setting this eqal to, we find the condition that + g ti (L ti () ρ i ) λ i g ti + γ i g ti =. (29) Ths, we see that we mst have the optimal as a linear combination of the g ti s, (t) = τ i g ti (t). (3) This has the effect of redcing the nonparametric problem to a problem of calclation of parameters in a finite dimensional space. Sbstitting into H, we have H(τ, λ, γ) = 1 2 τ Gτ τ G 2 τ τ G(a + b)+λ a τ Gλ + τ Gγ γ b + k, (31) where k is a constant that does not effect the location of the optimal point. The problem is now the following: max λ,γ sbject to positivity constraints on λ and γ. Calclating the derivative of H with respect to τ, we have where τ is optimal. Solving for τ,wehave min H(τ, λ, γ), (32) τ H τ = Gτ + G 2 τ G(a + b) Gλ + Gγ =, (33) τ =(I + G) 1 (a + b + λ γ). (34)

11 Path Planning 247 Now sbstitting this into H, we have H (τ,λ,γ)= 1 2 (a + b + λ γ) G(I + G) 2 (a + b + λ γ) (a + b + λ γ) G 2 (I + G) 2 (a + b + λ γ) (a + b + λ γ) (I + G) 1 G(a + b) (a + b + λ γ) (I + G) 1 Gλ +(a + b + λ γ) (I + G) 1 Gγ + λ a γ b + k = 1 2 (a + b + λ γ) G(I + G) 1 (a + b + λ γ) (a + b + λ γ) G(I + G) 1 (a + b + λ γ) + λ a γ b + k = 1 2 (a + b + λ γ) G(I + G) 1 (a + b + λ γ) +(λ γ) (I + G) 1 G(a + b)+k + k 2 + λ a γ b = 1 ( (I + G) 1 2 ( λ G (I + G) 1 )( ) G λ γ) (I + G) 1 G (I + G) 1 G γ +(λ, γ ) ( (I + G) 1 G(a + b)+a (I + G) 1 G(a + b) b ). It remains to solve the qadratic programming problem { ( 1 (I + G) 1 max λ,γ 2 ( λ G (I + G) 1 G γ) (I + G) 1 G (I + G) 1 G +(λ, γ ) ( (I + G) 1 G(a + b)+a (I + G) 1 G(a + b) b )}. )( ) λ γ (35) We have fond in nmerical simlation that this problem is easily solved and prodces splines which are qite well behaved. Althogh the qadratic programming problem is more complicated in terms of the matrices, these formlations seem to offer enogh improvement over the formlation of Problem 2 to be worthwhile. The next problem that we consider is the problem of constrcting monotone splines. This problem, as we discssed in the beginning of this section, is very important for many practical applications. We will do less than constrct monotone splines here, althogh it is possible to extend the techniqes we are sing to prodce an infinite dimensional qadratic programming problem that prodces monotone splines. We do not make that extension in this paper, bt restrict orselves to ensring that the spline in nondecreasing at each node. This problem has a significant increase in difficlty over the problem we have considered to this point. Problem 5. Let J() =ρ and let a set of constraints be imposed as 2 (t) dt + w i (L ti () α i ) 2, DL ti (), i =1,...,N. The problem then becomes simply sbject to the constraints and with min J(), L 2 [,T].

12 248 C. F. Martin et al. We define the Hamiltonian to be H(, λ) =J()+ w i 2 (L t i () α i ) 2 λ i DL ti (). (36) As before, the idea is to minimize H over and to maximize H over all positive λ. The scheme is to constrct the control that minimizes H as a fnction of λ and to se this parameterized control to convert the Hamiltonian to a fnction of a finite set of parameters. The reslting Hamiltonian will then be minimized with respect to a sbset of the parameters and the Hamiltonian will be redced to a fnction of the λ alone. Then the problem redces to a qadratic programming problem which can be solved sing standard software. We will se the notation h ti (s) = d dt g t i (s) for d dt g t(s), t=ti and we can then rewrite the Hamiltonian as H(, λ) = 1 ( ) 2 d (s) λ i 2 dt g t i (s)(s) ds + w i (L ti () α i ) 2. (37) We calclate the Gateax derivative of H with respect to to obtain ( ) D H(, λ)(w) = (s) λ i h ti (s)+ w i (L ti () α i ) g ti (s) w(s) ds, (38) and ths the optimal mst satisfy Ths, the optimal can be represented as (s) λ i h ti (s)+ w i (L ti () α i ) g ti (s) =. (39) (s) = λ i h ti (s)+ τ i g ti (s), (4) and the representation is niqe provided that for each i, the fnctions h ti and g ti are linearly independent. This redces to the condition A n 1 and with the conditions of eqation (3) that A. So we may assme that withot loss of generality, the representation is niqe. We now sbstitte into H to obtain the Hamiltonian as a fnction of τ and λ. We first establish some notation which we will need in order to simplify the formlation of the Hamiltonian. Let H =(h ij ), h ij = K =(k ij ), k ij = G =(g ij ), g ij = h ti (s)h tj (s) ds, h ti (s)g tj (s) ds, g ti (s)g tj (s) ds.

13 Path Planning 249 We sbstitte the expression for into H(, λ) and obtain after considerable simplification H(τ,λ)= 1 [ λ Hλ + λ Kτ + τ K λ + τ Gτ ] λ Hλ λ Kτ 2 w i [ + λ K e i e i Kλ + τ Ge i e i Gτ + αi 2 +2λ K e i e i Gτ 2 2α i λ K ] e i 2α i τge i. (41) We rewrite this after frther simplification as H(τ,λ)= 1 2 λ Hλ τ Gτ λ K DKλ τ GDGτ α Dα + λ KDGτ λ K Dα τ GDα, (42) where D is the diagonal matrix of weights w i. We now calclate the Gateax derivative of H with respect to τ and obtain D τ H(τ,λ)(w) =w ( Gτ + GDGτ + GDK λ Gα ). (43) Setting this eqal to zero, we have that the optimal τ mst satisfy the eqation or eqivalently since G is invertible (G + GDG)τ = Gα GDKλ, (44) τ = ( D 1 + G ) 1 (α Kλ). (45) It is clear that when we sbstitte this into H, we have a qadratic fnction of λ and so the problem is redced to solving a qadratic programming problem. Some simplification is possible, bt the overall form of the matrices involved seem to be qite messy. In general, H(λ) =λ F 1 λ + F 2 λ + F 3 α. (46) It is easy enogh to generalize this constrction to inclde higher order derivative constraints. The only problem that arises is to ensre that the representation of the optimal control at (4) is niqe. In order to generalize the constrction to linear combinations of derivatives and even to different linear combinations at different points, this obstrction becomes qite severe. We wold anticipate that some of the same problems arise here as do in the case of Birkhoff interpolation. This remains an active area of investigation. This constrction does not garantee that the spline fnction is monotone, bt only that the fnction is nondecreasing at each node. In nmerical experiments, we have fond that by adding points we can create monotone splines sing this constrction. We have not at this point proved that the addition of a finite nmber of points sffices to prodce monotone splines. 7. DYNAMIC TIME WARPING Traditionally, statistics has dealt with discrete data sets. However, most statisticians wold agree that information is sometimes lost when data is considered to be point or vector data. In longitdinal stdies, it is clear that it is the record of an event that is important, not the individal measrements. For example, if one is stdying the growth of individals in an isolated commnity, it is not the heights at yearly intervals that are of interest bt the crve that represents the growth of an individal over a seqence of years. These matters are discssed at length in

14 25 C. F. Martin et al. the seminal book of Ramsay and Silverman [6]. This book makes a very convincing argment for the stdy of crves as opposed to discrete data sets. Often when stdying crves, it is not clear that the independent variable (which we will refer to as time) is well defined. In the paper by Li and Ramsay [5], several examples are considered which make this point qite well. The first and second athor of this paper have seen this problem when trying to constrct weight crves for prematre babies the time of conception is seldom known exactly, and different ethnic stocks may have different growth crves. This leads to the problem of dynamic time warping or crve registration in order to compare crves that have different bases of time. In this section, we will follow the development of Li and Ramsay [5] and the development of Ramsay and Silverman [6]. We will show that their formlation is eqivalent to the problem of optimal otpt tracking in control theory, and then will give a formlation that is somewhat better behaved from an optimization viewpoint. Consider a set of crves DC = {f i (t) :i =1,...,N}, and assme that there is some commonality among the crves; i.e., they are all growth crves. Choose one sch crve, say f (t). The choice of this crve is discssed in some detail in [6]. Let T = {x α (t) :x α (t) is time-like }. What we mean by time-like is qite vage. We wold like for the fnctions to be at least almost monotone and convexity wold be a good property althogh at this point we do not want to impose too many conditions. We can pose the problem in the following manner: min f (t) f i (x α (t)). α This problem, althogh elegant in its simplicity, is too general to solve. In [5], the set of psedotimes is constrcted in a very clever and insightfl manner. They impose the conditions that ẍ ẋ = (t), with (t) being small, the idea being that this will make the crvatre of the time x(t) small and that the reslting fnction x(t) wold be time-like. They prodce very convincing reslts sing this techniqe. We can reformlate the problem in the context of optimal control. (We emphasize that we are only reformlating their problem.) Problem 6. (See [5].) min 2 (t)+(f i (t) f (x(t))) 2 dt, sbject to the constraint ẍ = (t)ẋ. The qestion of initial data is problem dependent, and we will leave a complete discssion to a later paper. The problem can be solved with or withot the initial data being given. See the treatise of Polak [12] for a very complete treatment of these varios cases. Consider a control system with otpt, of the form ẋ(t) =f (x(t)) + (t)g(t), y(t) =h (x(t)), and let a crve z(t) be given which we wold like to follow as closely as possible with the crve y(t). The classical model following problem is to constrct so that the distance between y and z is minimized. This problem is often considered in the asymptotic sense, bt in reality it is the finite time domain that is the most important in almost all applications. (47)

15 Problem 7. sbject to the constraint that min Path Planning (t)+(z(t) y(t)) 2 dt, ẋ(t) =f (x(t)) + (t)g(t), y(t) =h(x(t)). It is clear that Problem 5 is a special case of Problem 6 by taking ( ) x z =, ẋ and then noting that ( ) ( ) 1 ż = z + z. 1 The soltion to this problem is given by the soltion to the corresponding Eler-Lagrange eqations which in this case is a nonlinear two point bondary vale problem. The nonlinearity comes from the fact that the differential eqation has a nonlinearity, i.e., in Problem 5 the eqation is bilinear and in Problem 6 the eqation is nonlinear affine. However, even if we choose a linear constraint, the Eler-Lagrange eqations are nonlinear becase the otpt fnction is a nonlinear fnction of the state. These problems are in general only solvable by nmerical methods. Here the book of Polak [12] becomes very sefl. 8. TRAJECTORY PLANNING The trajectory planning problem is a fndamental problem in aeronatics, robotics, biomechanics, and many other areas of application in control theory. The problem comes in two distinct versions. The most general version is of interest for atonomos vehicles or atonomos movement in general. There the complete rote is not known in advance and mst be planned on-line. This problem is far beyond what we can do with the relatively simple tools we have developed here. The version of the problem we will consider in this section is in contrast qite simple. We are given a seqence of target points and target times, and we are reqired to be close to the point at some time close to the target time. This is typical of sch problems as the path planning in air traffic control and many problems in indstrial robotics. The problem of being close in space is nicely solved by Problems 2 4, bt the problem of being close in time has been difficlt to resolve. The concept of dynamic time warping seems to be the tool that can resolve this problem. Neither the problem nor its soltion are trivial, and it is nfortnate that there does not appear to be an analytic soltion. The following formlation seems to be the best that can be done at the moment. We define an axiliary system which we will se as the psedotime. We have chosen the system to be linear rather than the more complicated nonlinear system of Li and Ramsay. Let ( ) 1 F =, g =(, 1), and h =(1, ). We then consider the system ż = Fz + gv, w = hz, with initial data. We then have the otpt w represented as w(t) =t + t (t s)v(s) ds, so that if is small w is indeed time-like. We now formlate the path planning problem in the following manner. (48)

16 252 C. F. Martin et al. Problem 8. Let J(, v) = 1 2 sbject to the constraints w i ( Lw(ti)() α i ) min v ( 2 (t)+ρv 2 (t) ) dt min J(, v), ż = Fz + gv, w = hz, τ i (w (t i ) t i ) 2, and with both ẇ (t i ),, v L 2 [,T]. There is no really clean soltion to this problem, bt we are able to present an algorithm which gives at least a sboptimal soltion. We first minimize with respect to (this is jst Problem 3) and we find that the optimal is of the form (t) = τ i g w(ti)(t), (49) where the τ i are chosen as in the soltion to Problem 3. Recall that the vector τ satisfies the matrix eqation (DG + ρi)τ = Dα, and hence the τ i are fnctions of the as of yet nonoptimal w(t i ). However, this choice of is optimal for any choice of the w(t i ). Sbstitting into the fnctional J, we redce J to a fnction of v alone. We are then faced with a highly nonlinear fnctional to be minimized sbject to a differential eqation constraint. That is, we now have an optimal control problem which a nonlinear cost fnctional and a very simple linear control system as the constraint. It is possible to write down the Eler-Lagrange eqations for this problem, bt they are somewhat intimidating. Instead we opt for a sboptimal soltion that has a chance of being calclated sing good nmerical optimization procedres. We assme that the control v will have the form v(t) = γ i (t i t) +, (5) where the fnction (t i t) + is the standard fnction of polynomial splines that is zero when t>t i and is t i t otherwise, and the γ i are to be determined. Calclating w(t) with this choice of v, we see that w is a cbic spline with nodes at the t i. Sbstitting w into the cost fnctional, we have that J is now a fnction of the N parameters γ i. We have redced the problem to the a finite dimensional optimization problem. At this point, we have not taken into accont the ineqality constraints on the derivatives of the w(t i ), and this is done by introdcing the Hamiltonian of Problem 5. Becase of the nonqadratic natre of the cost fnctional, the qadratic programming problem has become a nonlinear programming problem, and as sch presents more difficlt nmerical problems. In a ftre paper, we will develop nmerical algorithms for the soltion of the problem.

17 Path Planning CONCLUSIONS In this paper, eight different problems originating from optimal control theory, statistics, and nmerical analysis were investigated. We showed that these problems cold be addressed within a nified framework, based on the relationship between optimal control and conventional or smoothing splines. The first five problems concerned the minimization of a qadratic cost fnctional sbject to a set of linear constraints, ranging from exact interpolation or penalized deviations from the nodes, to splines that pass throgh intervals or are nondecreasing at the nodes. For these problems, we were able to come p with explicit soltions. In Problems 6 and 7, the crve registration problem was addressed by extending or optimal control formlation to inclde the concept of dynamic time warping. The last problem concerned trajectory planning where we, given a set of target points and target times, wanted to be close to the points at times close to the target times. We were able to formlate these last problems within or optimal control framework in order to nite them with the previos problems into one nified theory. REFERENCES 1. Z. Zhang, J. Tomlinson and C. Martin, Splines and linear control theory, Acta Appl. Math. 49 (1), 1 34, (1997). 2. Z. Zhang, J. Tomlinson, P. Enqvist and C. Martin, Linear control theory, splines and approximation, In Comptation and Control III, (Edited by K. Bowers and J. Lnd), pp , Birkhäser, Boston, MA, (1995). 3. M. Egerstedt and C. Martin, Trajectory planning for linear control systems with generalized splines, In Proceedings of MTNS, Padova, Italy, (1998). 4. S. Sn, M. Egerstedt and C. Martin, Control theoretic smoothing splines, IEEEE Tran. At. Conf. (to appear). 5. X. Li and J. Ramsay, Crve registration, J. R. Stat. Soc. Ser. B Stat. Methodol. 6 (2), , (1998). 6. J. Ramsay and B. Silverman, Fnctional Data Analysis, Springer Series in Statistics, New York, (1997). 7. N. Agw and C. Martin, Optimal control of dynamic systems: Application to spline approximations, Applied Mathematics and Comptation 97, , (1998). 8. N. Agw, Optimal control of dynamic systems and its application to spline approximation, Ph.D. Dissertation, Texas Tech University, (1996). 9. D. Lenberger, Optimization by Vector Space Methods, John Wiley & Sons, New York, (1969). 1. E. Wegman and I. Wright, Splines in statistics, J. Amer. Statist. Assoc. 78, , (1983). 11. G. Wahba, Spline models for observational data, In CBMS-NSF Regional Conference Series in Applied Mathematics, Volme 59, Society for Indstrial and Applied Mathematics (SIAM), Philadelphia, PA, (199). 12. E. Polak, Optimization. Algorithms and Consistent Approximations, In Applied Mathematical Sciences, Vol. 124, Springer-Verlag, New York, (1997).