Geometric Motion Planning for Affine Control Systems with Indefinite Boundary Conditions and Free Terminal Time
aa r X i v : . [ ee ss . S Y ] J a n Geometric Motion Planning for Affine Control Systems with IndefiniteBoundary Conditions and Free Terminal Time
Shenyu Liu, Yinai Fan Mohamed-Ali Belabbas
Abstract — The problem of motion planning for affine controlsystems consists of designing control inputs that drive a systemfrom a well-defined initial to final states in a desired amountof time. For control systems with drift, however, understandingwhich final states are reachable in a given time, or reciprocallythe amount of time needed to reach a final state, is often themost difficult part of the problem. We address this issue in thispaper and introduce a new method to solve motion planningproblems for affine control systems, where the motion desiredcan have indefinite boundary conditions and the time requiredto perform the motion is free. The method extends on our earlierwork on motion planning for systems without drift. A canonicalexample of parallel parking of a unicycle with constant linearvelocity is provided in this paper to demonstrate our algorithm.
I. I
NTRODUCTION
Due to its ubiquity in control applications ranging fromrobotics to autonomous wheeled vehicles, motion planninghas been widely studied (see, e.g., [1], [2] and the referencetherein) and a host of methods have been developed. Froma theoretical point of view, the Chow-Rashevski Theoremprovides us with conditions under which a driftless non-holonomic system is controllable [3]. the idea of small timelocal controllability can be generalized to affine systemswith drift as studied in [4], [5]. Beyond that the motionplanning is still quite challenging and even the generalcase of controllability of nonlinear systems is still an openproblem. Nevertheless, for some specific nonlinear systemswith drift, motion planning or control algorithms are givenin [6], [7], [8], [9].Inspired by the curving shortening flow as studied in[10], we have proposed a geometrical approach for motionplanning in [11] for driftless affine systems. The methodworks by “deforming” an arbitrary path between a giveninitial state x i and a given final state x f into an almostadmissible trajectory for the system, from which we canextract the controls u ∗ that drive the system from x i to x f approximately. Using a variational approach, we provided aproof of convergence of the method as well. By encodingobstacles into barrier functions, we are able to plan themotion while avoid obstacles. In the later work [12], wemodified the algorithm so it is capable of addressing motionplanning problems for affine systems with drift and inputconstraints by solving the so called affine geometric heatflow equation.Nevertheless, most literature mentioned above includingour previous works only focused on motion planning prob-lems with both end points of the motion as well as thetime span fixed. In practice, we very often allow indefiniteboundary conditions (IBCs) and free terminal time (FTT). It is appreciated that in many motion planning problems thereis no need to fix the boundary conditions as a priority, orthe optimal/feasible boundary states need to be determinedin the motion planning problems. For example, consider thetask of somersault performed by a robot made of linkagesas studied in [13], [14]. While the final joint angles areprescribed, there are no constraints on the final velocities ofthe joints if “landing with impact” is allowed. On the otherhand, if the final dynamic configuration is fully fixed for thesame somersault problem, we have implicit constraints onchoosing the initial configuration because of the conservationof angular momentum in mid-air and hence it is better to askthe algorithm to find the feasible initial configuration [15].In terms of terminal time, it is less important for driftlesssystems because the time span can always be adjustedby input scaling; however, scaling will not work if thereis drift or the control is constrained. In other words, thereachable space depends on the terminal time, which affectsthe feasibility of our motion planning problem so FTT isindeed crucial in our case and the terminal time cannot befixed prior to the motion planning problem either.By deploying some ideas from calculus of variation andstate augmentation, in this work we modify our previousalgorithm so that it is able to solve motion planning problemswith both IBCs and FTT. The paper has 6 sections. InSection II our motion planning problem is formulated withnecessary preliminaries. We then state the changes in ouralgorithm for handling IBCs in Section III, followed by thediscussion of FTT in Section IV. Our algorithm is thenexamined in a case study on unicycle in Section V and thepaper is concluded in Section VI.II. P ROBLEM FORMULATION AND PRELIMINARIES
Consider an affine control system with drift: ˙ x = h ( x ) + F ( x ) u (1)Where x ∈ R n are the states, and for each fixed t and x , h ( x ) ∈ R n is the drift and F ( x ) ∈ R n × m consists ofcolumns of control directions and u ( t ) ∈ R m is the controlinput. We further assume m < n so the system is under-actuated and F ( x ) is full rank for all x ∈ R n . Let T > be the terminal time, either free or fixed. For simplicity weassume that both the functions h ( · ) , F ( · ) are smooth andthe input function u ( · ) ∈ L ([0 , T ] → R m ) =: U . Fora curve x ( · ) ∈ C ([0 , T ] → R n ) =: X , Let L be someLagrangian defined with respect to x ( t ) , ˙ x ( t ) . Defined the ction functional A over X as follows: A ( x ) := Z T L ( x ( t ) , ˙ x ( t )) dt (2)It is well known that Euler-Lagrange equation ∂L∂x ( x ∗ ( t ) , ˙ x ∗ ( t )) − ddt ∂L∂ ˙ x ( x ∗ ( t ) , ˙ x ∗ ( t )) = 0 ∀ t ∈ [0 , T ] (3)is a necessary condition for a curve x ∗ to be a local optimizerof (2) in X . Instead of solving the system of ODEs (3)directly, we can view the solution of (3) as the steadystate solution of a system of PDEs by adding one morevariable s to the curve x so now it becomes x ( t, s ) , definedon [0 , T ] × [0 , ∞ ) . Notice that for each fixed s , x ( · , s ) still represents a curve in X . As x ( t, s ) is a multivariablefunction, we use x t and x s to represent ∂x∂t , ∂x∂s , respectively.We define the affine geometric heat flow (AGHF) equationas follows: x s = G ( x ) − (cid:18) ddt ∂L∂x t ( x, x t ) − ∂L∂x ( x, x t ) (cid:19) , (4)where G is a positive definite n × n matrix which will begiven later. Unlike the case we have studied in our earlierwork that both ends of the curve is fixed in the sense thatthere exists x i , x f ∈ R d such that we have the boundaryconditions x (0 , s ) = x i , x ( T, s ) = x f ∀ s ≥ , we now only fix part of them in the case of IBCs. To bemore precise, let S bc ⊂ S := { , · · · , d } × { , T } . For somegiven x bc ( i, t ) ∈ R n , ( i, t ) ∈ S bc , x i ( t, s ) = x bc ( i, t ) ∀ ( i, t ) ∈ S bc , s ≥ (5)We allow the case that S bc = ∅ , meaning that there are noboundary conditions at all. Meanwhile, in order to solve theparabolic type PDE (4), we still need to feed it with an initialcurve x ( t,
0) = z ( t ) , t ∈ [0 , T ] (6)where z ( · ) ∈ X ′ with X ′ := { x ∈ X : x i ( t ) = x bc ( i, t ) ∀ ( i, t ) ∈ S bc } In other words, X ′ is the set of all C curves with the IBCssatisfied. Note that the curves in X ′ do not need to meet thesystem dynamics (1). We also define the set of admissiblepaths by X ∗ := { x ∈ X ′ : (1) is satisfied for some u ∈ U} We say the motion planning problem is feasible when X ∗ = ∅ . While finding a curve in X ′ is trivial, finding an admissiblepath in X ∗ is difficult. In this work we will show an efficientalgorithm that gives an approximation to some x ∗ ∈ X ∗ when the problem is feasible. III. I NDEFINITE BOUNDARY CONDITIONS
We first consider the case when the terminal time T isfixed. Notice that when S bc = S , we recover the fully con-strained boundary conditions. However, when S bc is a propersubset of S , algebraically there are not enough boundaryconditions in (5) so the ODEs (3) are under-determined.Nevertheless, For being a minimizer of the functional (2),it should also give variation with respect to perturbationsin the free boundary states; In other words, we should alsohave ∂L∂ ( x t ) i ( x ( t, s ) , x t ( t, s )) = 0 ∀ ( i, t ) ∈ S \ S bc , s ≥ (7)(7) together with (5) are the new boundary conditions wewill use for solving the AGHF equation (4). A. Decreasing action functional
At this point we provide a lemma showing that the steadystate solution of (4) is a solution of (3).
Lemma 1 (Gradient decent rule under IBCs)
Let x ( t, s ) be a solution to the AGHF (4) with an initial condition (6) and boundary conditions (5) , (7) . Then ∂ A ( x ( · ,s )) ∂s ≤ andit is if and only if (3) is satisfied on the curve x ( · , s ) . With some modification on the boundary conditions, its proofis essentially the same as the proof of Lemma 1 in [12] andprovided in the appendix.As a remark, similar arguments also hold if we require theindefinite boundary states not fully free but in some subsetsof R n . In other words, if the motion planning problem needsto satisfy the boundary condition that x (0) ∈ Ω i := { x ∈ R n : φ ij ( x ) = 0 , j = 1 , · · · , n i } ,x ( T ) ∈ Ω f := { x ∈ R n : φ fk ( x ) = 0 , k = 1 , · · · , n f } , then all we need are some transversality conditions suchthat ∂L∂x t ( x (0 , s ) , x t (0 , s )) is a linear combination of ∇ φ ij ’sand ∂L∂x t ( x ( T, s ) , x t ( T, s )) is a linear combination of ∇ φ fk ’sin the non-degenerate case. The proof is similar and henceomitted. For more information on transversality conditionsfor optimization, see, e.g., [16]. B. Algorithm
Our algorithm for motion planning with IBCs and fixedterminal time is very similar to our old algorithm for fixedboundary states, consisting of the following steps:S1: Find a bounded n × ( n − m ) x -dependent matrix F c ( x ) ,differentiable in x , such that ¯ F ( x ) := (cid:0) F c ( x ) | F ( x ) (cid:1) ∈ R n × n (8)is invertible for all x ∈ R n . The matrix F c ( x ) can beobtained using, e.g., the Gram-Schmidt procedure.S2: Evaluate G ( x ) := ( ¯ F ( x ) − ) ⊤ D ¯ F ( x ) − (9)here D := diag ( λ, · · · , λ | {z } n − m , , · · · , | {z } m ) for some suffi-ciently large λ > and set L ( x, ˙ x ) = ( ˙ x − h ( x )) ⊤ G ( x )( ˙ x − h ( x )) . (10)S3: Solve the AGHF (4) with boundary conditions (5), (7)and initial condition (6). Denote the solution by x ( t, s ) ;S4: For some sufficiently large ¯ s , Evaluate the extractedcontrol by u ( t ) := (cid:0) I m × m (cid:1) ¯ F ( x ( t, ¯ s )) − ( x t ( t, ¯ s ) − h ( x ( t, ¯ s )) (11) Output:
The control u ( · ) obtained in (11) yields a trajectory ˜ x ( · ) when integrating (1) from the initial value ˜ x (0) = x (0 , ¯ s ) , which is our solution to the motion planning prob-lem. We call it integrated path .Again, as we stated in [12], the integrated path from thisalgorithm will only be an approximated admissible path andhow close the end states to the desired boundary conditionsdepends on how large are the two parameters λ and ¯ s . Inaddition, this algorithm will work only if the motion planningproblem is feasible and the initial curve v is chosen in someneighborhood of an admissible path, which is not pre-known.The theoretical result is almost the same as our previous oneso we only provide the statements but omit the proof here. Theorem 1
Consider the system (1) and assume the motionplanning problem with IBCs (5) and fixed terminal time T is feasible. Then there exists C > such that for any λ > ,there exists an open set Ω λ ⊆ X ′ so that as long as theinitial curve z ∈ Ω λ , The integrated path ˜ x ( · ) from ouralgorithm with sufficiently large ¯ s has the properties that forall ( i, ∈ S bc , ˜ x i (0) = x bc ( i, (12) and for all ( i, T ) ∈ S bc , | ˜ x i ( T ) − x bc ( i, T ) | ≤ r T M Cλ exp (cid:18) T L T + L C ) (cid:19) . (13) where L , L are the Lipschitz constants of h, F and M is the upper bound of k F c k , which is constructed in thealgorithm S1.C. Comments on implementation in matlab Our algorithm is implemented in matlab . The first andsecond step of the algorithm is processed symbolically. Step3 is implemented via the commend pdepe , where the typeof hyperbolic PDEs it is capable of solving is of the form c ( t, s, x, x t ) x s = t − m ∂∂t ( t m f ( t, s, x, x t )) + s ( t, s, x, x t ) (14)By comparison with (4), we see that in our case we simplyneed m = 0 , c = G, f = ∂L∂x t and s = − ∂L∂x , which can alsobe done symbolically. In addition, the boundary conditionsare formulated in the form p ( t, s, x ) + q ( t, s ) f ( t, s, x, x t ) = 0 where the f is the same as in (14), and thus is ∂L∂x t in ourcase. Therefore for the boundary conditions (5) and (7), theimplementation is simply p i ( t, s, x ) = x i ( t, s ) − x bc ( i, t ) , q i ( t, s ) = 0 , ∀ ( i, t ) ∈ S bc , s ≥ p i ( t, s, x ) = 0 , q i ( t, s ) = 1 , ∀ ( i, t ) ∈ S \ S bc , s ≥ . IV. F
REE TERMINAL TIME
For a driftless affine system, If u ( · ) : [0 , → R m gives an admissible path satisfying the boundary condition x (0) = x i , x (1) = x f , then T u (cid:0) tT (cid:1) gives a time scaledadmissible path with x ′ (0) = x i , x ′ ( T ) = x f . In otherwords, there is no need to consider the terminal time T otherthan since it can always be done by scaling the inputs.However, when there are input constraints or in the presenceof the drift term, such scaling is no longer true and hencethe minimization of A in (2) as studied in Lemma 1 shouldbe with respect to T as well. In other words, unlike thedriftless case where the reachable space of a driftless systemis independent of T , the reachable space of an affine systemwith drift or constrained inputs is somehow related to theterminal time T . For example, the unicycle with unit linearvelocity can only reach planar positions within the ball ofradius T at time T . Thus we cannot simply just fix a random T prior to minimizing A , in which case solutions may evennot exist. A. Augmenting the true time
Compared with the case of fixed terminal time, FTT in theview of maximum principle means that the Hamiltonian isidentically along the optimal trajectory. That informationis not helpful in our case, as we do not rely our analysison the costates nor Hamiltonian. Instead, while we stillconsider functions defined over a fixed domain [0 , , theway to tackle FTT is to augment a new state τ ∈ R tothe system, which is the true time variable that starts from τ (0) = 0 and τ (1) = T yet to be determined. There isalso an additional constraint on the function τ ( · ) that itneeds to be strictly increasing, in which case the inversefunction τ − exists and we can recover the control as afunction of the true time from u ( · ) by u † ( t ) = u ( τ − ( t )) .For smooth τ ( · ) , this monotonicity constraint can be resolvedby deploying our earlier technique on constrained inputs bytreating the derivative of τ as another extra state, or simplywe define ˙ τ ( t ) = a ( t ) , ˙ a ( t ) = u ( t ) where u is theadditional input to the twice-augmented system. Notice thatsince τ is the true time, dxdτ should obey the true systemdynamics (1) instead of dxdt . Thus using chain rule, we have ˙ x := dxdt = dxdτ dτdt = h ( x ) a + F ( x ) a u . In summary, denotethe augmented state x ′ = xτa , (15)e have ˙ x ′ = ˙ x ˙ τ ˙ a = h ( x ) a a | {z } h ′ ( x ′ ) + F ( x ) a
00 00 1 | {z } F ′ ( x ′ ) (cid:18) auu (cid:19) (16)By this augmentation we have new drift term h ′ and newadmissible control direction matrix F ′ . The reason why wetake one a out from F ′ and multiply it to the control u will be explained later when we discuss the total energyconsumption of the planned path. In addition, by observationwe see that if the inadmissible control direction matrix isconstructed by F ′ c ( x ′ ) := F c ( x ) 00 10 0 (17)then ¯ F ′ = ( F ′ c F ′ ) is full rank if ¯ F = ( F c F ) isfull rank as we needed earlier. Because the dimension ofthe system (16) is now n + 2 , D ′ , G ′ , L ′ should all bedefined accordingly. There are also some small tweaks onthe boundary conditions. We still have the old boundaryconditions (5), (7); we also have a new boundary conditionon τ because true time also starts at : τ (0 , s ) = 0 ∀ s ≥ . (18)We do not have any constraints on τ (1 , s ) , a (0 , s ) , a (1 , s ) ;nevertheless, according to the previous discussion on IBCswe should have ∂L∂τ t (¯ x (1 , s ) , ¯ x t (1 , s ) ,
1) = 0 ,∂L∂a t (¯ x (0 , s ) , ¯ x t (0 , s ) ,
0) = 0 ,∂L∂a t (¯ x (1 , s ) , ¯ x t (1 , s ) ,
1) = 0 (19)for all s ≥ as a complement in order to solve ourAGHF. The rest can be proceeded similarly to the algorithmin Section III-B and we summarize it in the followingsubsection: B. Algorithm for FTT problem
S1: Augment the states as in (15). Find a bounded n × ( n − m ) x -dependent matrix F c ( x ) , differentiable in x ,such that ¯ F defined in (8) is invertible for all x ∈ R n .Denote ¯ F ′ ( x ′ ) = ( F ′ c ( x ) F ′ ( x ′ )) where F ′ , F ′ c comefrom (16), (17).S2: Evaluate G ′ ( x ′ ) := ( ¯ F ′ ( x ′ ) − ) ⊤ D ′ ¯ F ′ ( x ′ ) − (20)where D ′ = diag ( λ, · · · , λ | {z } n − m +1 , , · · · , | {z } m +1 ) and set L ′ ( x ′ , ˙ x ′ ) = ( ˙ x ′ − h ′ ( x ′ )) ⊤ G ′ ( x ′ )( ˙ x ′ − h ′ ( x ′ )) . (21)S3: Pick some T g > , a ig , a fg ∈ R as the initial guess for T, a (0) , a (1) . Let z ′ ∈ C ([0 , → R n +2 ) be an initialcurve such that z ′ i ( t ) = x bc ( i, t ) for all ( i, t ) ∈ S bc , z ′ n +1 (0) = 0 , z ′ n +1 (1) = T g , z ′ n +2 (0) = a ig , z ′ n +2 (1) = a fg . Solve the AGHF (4) with boundary conditions (5),(7), (18), (19) and initial curve z ′ described above.Denote the solution by x ′ ( t, s ) .S4: Fix ¯ s sufficiently large. Define w ( t ) := ¯ F ′ ( x ( t, ¯ s )) − ( x ′ t ( t, ¯ s ) − h ′ ( x ′ ( t, ¯ s )) . Split w so w ⊤ = ( v ⊤ v u ⊤ u ) for some v ∈ C ([0 , → R n − m ) , u ∈ C ([0 , → R m ) , v , u ∈ C ([0 , → R ) . Define ˜ τ ( s ) = Z s a ( t ) dt (22)where a ( t ) = Z t u ( r ) dr + x ′ n +2 (0 , ¯ s ) = x ′ n +2 ( t, ¯ s ) , (23)then T = ˜ τ (1) is our resultant terminal time and u † ( t ) = u (˜ τ − ( t )) a (˜ τ − ( t )) is our extracted control. Output:
The integrated path ˜ x † is obtained by integrating(1) with the extracted control u † ( · ) and initial value ˜ x † (0) =( x ′ (0 , ¯ s ) x ′ (0 , ¯ s ) · · · x ′ n (0 , ¯ s )) ⊤ . C. Cost minimizing
We provide a theorem in the FTT case here, which issimilar to Theorem 1. Its proof is contained in the appendix.
Theorem 2
Consider the system (1) and assume the motionplanning problem with IBCs (5) and FTT is solvable. Theintegrated path ˜ x † ( · ) from our algorithm with properly cho-sen initial curve z ′ and sufficiently large ¯ s has the propertiesthat for all ( i, ∈ S bc , ˜ x † i (0) = x bc ( i, (24) and there exists K > such that for all ( i, T ) ∈ S bc , | ˜ x † i ( T ) − x bc ( i, T ) | ≤ K √ λ . (25)Next we provide a heuristic argument that our extractedcontrol is “economical”. Plug x ′ ( t, ¯ s ) which is derived fromStep 3 into L ′ defined in Step 2, we have L ′ ( x ′ ( t, ¯ s ) , x ′ t ( t, ¯ s )) = w ( t ) ⊤ D ′ w ( t )= λ | v ( t ) | + λv ( t ) + | u ( t ) | + u ( t ) ≥ | u ( t ) | , From our analysis of Lemma 1 we see that the actionfunctional A = R L ′ dt is minimized when solving theAGHF; in other words, the L -norm of u ( · ) is relativelysmall from our algorithm. On the other hand, again by thechange of variable via ˜ τ − ( r ) = s , this L -norm is exactlythe energy of the actual input: E = Z T | u † ( r ) | dr = Z T | u (˜ τ − ( r )) | a (˜ τ − ( r )) dr = Z | u ( s ) | ds his in fact is not a coincidence; it is only achieved when wedesign the F ′ ( x ′ ) in that particular form as in (16) where wehave shifted one a to the input. As a summary, our algorithmnot only finds an approximation to an admissible path whichsatisfying the IBCs with FTT, the energy consumption of theplanned path is also relatively small.V. C ASE STUDY : UNICYCLE WITH UNIT LINEARVELOCITY
A planar unicycle has three state variables, x, y whichrepresent its planar position and θ which represents itsorientation. The kinematics of a unicycle with unit linearvelocity is given by ˙ x ˙ y ˙ θ = cos θ sin θ + u, (26)from which we have h = (cid:0) cos θ sin θ (cid:1) ⊤ and F = (cid:0) (cid:1) ⊤ . It is not hard to see that F c = isthe orthogonal complement to F . Thus according to ouralgorithm we have h ′ ( x ′ ) = a cos θa sin θ a , ¯ F ′ ( x ′ ) = a
00 0 1 0 00 0 0 0 1 . Consider the canonical parallel parking problem that ( x, y, θ ) | τ =0 = (0 , , and ( x, y, θ ) | τ = T = (0 , , where T is to be determined. We would also like to ensure thatthe energy cost E = R T u ( τ ) dτ is small. We pick λ =1000 , T g = 10 , a ig = a fg = 1 and Let z ′ be the line segmentconnecting the boundary conditions for our algorithm.The evolution of the ( x, y, θ ) coordinates in the AGHFsolution x ′ ( s, t ) with respect to s is shown in Figure 1. Noticethat in Figure 1a we have s = 0 and hence it is indeed thefirst three coordinates of the initial curve z ′ . In Figure 1d, thecurve barely changes any more with respect to s so the PDEsolver is stopped and we use ¯ s = 1 to extract the control.The extracted control is shown as the black curve in Figure 2and integrated path is shown as the black curve in Figure 3.It turns out that by our algorithm T = 1 . and E =21 . . As a comparison, a heuristic admissible path forthe unicycle system (26) which consists of two semicirclesis considered. Such a path has a total length of π , and hencethe total traveling time is also π because of unit velocity.By observation we see that u , or the turning rate, is equalto the curvature of the path and thus u ( t ) = 4 for the firsthalf and u ( t ) = − for the second half. As a result, thetotal energy cost in this case is × π ≈ . . Both thetotal time and energy cost of this heuristic path is larger thanwhat we derived from our proposed algorithm. In addition,we also applied our motion planning algorithm studied in[12] for fixed terminal time with varies T . Their extracted − − . . − . . . . . xy θ (a) s = 0 − − . . − . . . . . xy θ (b) s = 0 . − − . . − . . . . . xy θ (c) s = 0 . − − . . − . . . xy θ (d) s = 1 Fig. 1: The ( x, y, θ ) plots of the solution of (4) for differentvalues of s . . . . . . . . . . . . . − − − − − t u OptimalHeuristicT=1.2T=1.3T=1.5T=2T=3
Fig. 2: The extracted control for the FTT algorithm, theheuristic control and the extracted controls from our previousalgorithm with fixed T . . − . − . . . . . . . . x y OptimalHeuristicT=1.2T=1.3T=1.5T=2T=3
Fig. 3: The integrated path for the FTT algorithm, theheuristic path and the integrated paths from our previousalgorithm with fixed T .controls and integrated paths are also illustrated in Figure 2and Figure 3.The energy costs of these different results are shown inFigure 4. Notice that this motion planning problem has nosolution with global minimal energy cost. This is becausethe parallel parking of the constant linear velocity unicyclecan always be accomplished by an S-shaped curve witharbitrarily large turning radius r , as seen by the trend ofsolutions with larger T in Figure 3. By doing that themagnitude to the control is approximately O (1 /r ) while thetotal time is approximately O ( r ) and hence E = R T | u | dt = O (1 /r ) → as r increases to infinity. This asymptoticbehavior is also reflected on the plot of energy in Figure 4.However finding globally optimal or sub-optimal solution forthe motion planning is out of our interest because it requiresinfinite or extremely long travel time of the unicycle andhence impractical. On the other hand, our FTT algorithm isable to produce a value of T which gives us a local optimalsolution; in addition, this T and energy cost coincide withthe local optimal result generated by iterations of the motionplanning algorithm with fixed terminal time.As a final remark, we want to comment on finding thetrue optimal solution to this motion planning problem viamaximum principle. We start by formulating the Hamiltonianas H = p ⊤ f − L = p cos( θ ) + p sin( θ ) + p u − u (27)Now by maximum principle we have u = p , p , p areconstants and ˙ p = p sin( θ ) − p cos( θ ) . In addition, sincethe problem has a free terminal time, we have H ≡ . Whilewe still have boundary conditions on the states x, y, θ , Wedo not know the value of p , p and there are neither anyboundary conditions for p . On the other hand, if we take . . . . . T E Optimal E for each fixed T Optimal E from FTT algorithmHeuristic E Fig. 4: Energy vs. T .the second derivative of θ , we see that ¨ θ = dudθ = 12 dp dt = p θ ) − p θ ) , (28)which is a second order nonlinear ODE. While there are nogeneral solutions for nonlinear ODEs with IBCs and FTT,the general solution of (28) can be expressed in terms ofJacobi elliptic functions [17], which requires quite a lot ofwork. It can also be seen that finding the exact optimalpath via maximum principle is difficult to be generalized formore complicated systems or in higher dimensions. On thecontrary, in trade of the true minimal cost and the accuracyof the exact expression of the optimal solution, our algorithmis quite systematic and usually gives a good approximatingsolution within reasonable amount of computation time.VI. C ONCLUSION
In this paper we have extended our earlier method formotion planning for affine control systems with fixed bound-ary conditions and fixed terminal time to indefinite boundaryconditions and free terminal time. We have first shown thatthe deficiency of boundary conditions can be completed byconstraints on the Lagrangian with respect to the derivative ofthe corresponding states and it was verified via a perturbationargument. On the other hand, a time scaling has beenapplied to the motion planning problem, which resulted in anaugmented affine system with drift and indefinite boundaryconditions and hence the motion with indefinite terminal timecan be planned by appealing to the techniques we developedin the previous work and the analysis on indefinite boundaryconditions. In the end of the paper we have also studied acanonical example of unicycle with constant linear velocityand used our algorithm to show how parallel parking can beaccomplished in the most economical way.A
PPENDIX
A. Proof of Lemma 1
Firstly, by first order approximation we have x ( t, s + δ ) = x ( t, s ) + δx s ( t, s ) + o ( δ ) lug it into the first order variation of L , we have A ( x ( · , s + δ )) = Z T L ( x ( t, s + δ ) , x t ( t, s + δ )) dt = Z T L ( x ( t, s ) , x t ( t, s )) + ( δx s ( t, s ) + o ( δ )) ⊤ ∂L∂x + (cid:18) ddt ( δx s ( t, s ) + o ( δ )) (cid:19) ⊤ ∂L∂x t + o ( δ ) dt = A ( x ( · , s )) + δ Z T x s ( t, s ) ⊤ ∂L∂x + x ts ( t, s ) ⊤ ∂L∂x t dt + o ( δ ) , where all o ( δ ) terms are collected together. Use integrationby parts for the x ts ( t, s ) ⊤ ∂L∂x t term, we have A ( x ( · , s + δ )) = A ( x ( · , s )) + δ x s ( t, s ) ⊤ ∂L∂x t (cid:12)(cid:12)(cid:12)(cid:12) T + Z T x s ( t, s ) ⊤ ∂L∂x − x s ( t, s ) ⊤ ddt ∂L∂x t dt ! + o ( δ ) . Our new boundary conditions (5), (7) imply that x ⊤ s ∂L∂x t = 0 for both t = 0 , T and all s ≥ . Hence the integrated term x s ( t, s ) ⊤ ∂L∂x t (cid:12)(cid:12)(cid:12) T vanishes. Thus plug in the AGHF (4) here, ∆ A = δ Z T x s ( t, s ) ⊤ (cid:18) ∂L∂x − ddt ∂L∂x t (cid:19) dt + o ( δ )= − δ Z T G ( x ) | x s ( t, s ) | dt + o ( δ ) where ∆ A = A ( x ( · , s + δ )) − A ( x ( · , s )) and hence ∂ A ( x ( · , s )) ∂s = lim δ → ∆ Aδ = − Z T G ( x ) | x s ( t, s ) | dt ≤ and equality is achieved if and only if x s ( t, s ) = 0 almosteverywhere for t ∈ [0 , T ] . Because of (4) and the fact that x ( · , s ) ∈ C , (3) is satisfied on x ( · , s ) . (cid:3) B. Proof of Theorem 2
Note the property (24) is directly given by the construc-tion. The major difference between the two algorithms isin Step 4, where a time scaling is involved in the secondalgorithm on FTT. Nevertheless, if we directly feed ( u, u ) to the system (16) as required by the Step 4 in the firstalgorithm, by the results of Theorem 1 we again concludethe bounds of (13) for all ( i, T ) ∈ S bc , where ˜ x ( t ) = ˜ x † (0) + Z t h (˜ x ( s )) a ( s ) + F (˜ x ( s )) a ( s ) u ( s ) ds and a as defined in (23). On the other hand, the integratedpath is given by ˜ x † ( t ) = ˜ x † (0) + Z t h (˜ x † ( r )) + F (˜ x † ( s )) u † ( r ) dr = ˜ x † (0) + Z t h (˜ x † ( r )) + F (˜ x † ( r )) a (˜ τ − ( r )) u (˜ τ − ( r )) dr Set ˜ τ − ( r ) = s and notice that drds = d ˜ τ ( s ) ds = a ( s ) , we seethat ˜ x † ( t ) = ˜ x † (0) + Z ˜ τ − ( t )0 (cid:18) h (˜ x † (˜ τ ( s )))+ F (˜ x † (˜ τ ( s ))) a ( s ) u ( s ) (cid:19) a ( s ) ds = ˜ x † (0)+ Z ˜ τ − ( t )0 h (˜ x † (˜ τ ( s ))) a ( s ) + F (˜ x † (˜ τ ( s ))) a ( s ) u ( s ) ds which implies that ˜ x † ( t ) = ˜ x (˜ τ − ( t )) . In particular, ˜ x † ( T ) = ˜ x (1) and we conclude (25) from (13). (cid:3) R EFERENCES[1] J. P. Laumond, S. Sekhavat, and F. Lamiraux,
Robot Motion Planningand Control , J. P. Laumond, Ed. Springer, 1998.[2] S. M. LaValle,
Planning Algorithms . Cambridge University Press,2006.[3] W. Chow, “ber systeme von linearen partiellen differentialgleichungenerster ordnung.”
Mathematische Annalen , vol. 117, pp. 98–105,1940/1941. [Online]. Available: http://eudml.org/doc/160043[4] H. Sussmann, “A general theorem on local controllability,”
SIAMJournal on Control and Optimization , vol. 25, no. 1, pp. 158–194,1987.[5] H. J. Sussmann, “Local controllability and motion planning for someclasses of systems with drift,” in [1991] Proceedings of the 30th IEEEConference on Decision and Control , Dec 1991, pp. 1110–1114 vol.2.[6] A. D. Luca and G. Oriolo,
Modelling and Control of NonholonomicMechanical Systems . Springer, 1995, pp. 277–342.[7] J. . Godhavn, A. Balluchi, L. Crawford, and S. Sastry, “Path planningfor nonholonomic systems with drift,” in
Proceedings of the 1997American Control Conference , vol. 1, June 1997, pp. 532–536.[8] M. Reyhanoglu, A. van der Schaft, N. H. Mcclamroch, and I. Kol-manovsky, “Dynamics and control of a class of underactuated me-chanical systems,”
IEEE Transactions on Automatic Control , vol. 44,no. 9, pp. 1663–1671, Sep 1999.[9] S. Zeng, “Iterative optimal control syntheses illustrated on the brockettintegrator,” vol. 52, no. 16, 2019, pp. 138 – 143, 11th IFAC Sympo-sium on Nonlinear Control Systems NOLCOS 2019.[10] K.-S. Chou and X.-P. Zhu,
The curve shortening problem . CRC Pressbook, 2001.[11] M. A. Belabbas and S. Liu, “New method for motion planning fornon-holonomic systems using partial differential equations,” in , May 2017, pp. 4189–4194.[12] S. Liu, Y. Fan, and M.-A. Belabbas, “Affine geometric heat flow andmotion planning for dynamic systems,” vol. 52, no. 16, 2019, pp. 168 –173, 11th IFAC Symposium on Nonlinear Control Systems NOLCOS2019.[13] E. Papadopoulos, I. Fragkos, and I. Tortopidis, “On robot gymnasticsplanning with non-zero angular momentum,” in
Proceedings 2007IEEE International Conference on Robotics and Automation , April2007, pp. 1443–1448.[14] R. Shu, A. Siravuru, A. Rai, T. Dear, K. Sreenath, and H. Choset,“Optimal control for geometric motion planning of a robot diver,” in , Oct 2016, pp. 4780–4785.[15] Y. Fan, S. Liu, and M.-A. Belabbas, “Mid-air motion planning offloating robot using heat flow method,” vol. 52, no. 22, 2019, pp. 19– 24, 1st IFAC Workshop on Robot Control WROCO 2019.[16] D. Liberzon,
Calculus of Variations and Optimal Control Theory: AConcise Introduction . Princeton University Press, 2012, ch. 3.[17] E. T. Whittaker and G. N. Watson,