Optimal motion planning of wheeled mobile robots

Dottorato di Ri er a in Te nologie dell'Informazione

XXIVCi lo

Optimal motion planning

of

wheeled mobile robots

Coordinatore:

Chiar.moProf. Mar o Lo atelli

Tutor:

Chiar.moProf. Aurelio Piazzi

Dottorando: Gabriele Lini

January2012

sostegno.

Introdu tion 1

1 Minimum-time velo ity planning 5

1.1 Optimal ontrol theory . . . 7

1.1.1 Problemstatement andnotation . . . 7

1.2 Linear time-optimalproblem . . . 8

1.2.1 Themaximum prin iple . . . 9

1.2.2 Bang-bang prin iple for s alarsystems . . . 10

1.3 Minimum-time velo ity planning with arbitrary boundary on- ditions . . . 12

1.3.1 Problemstatement and the stru ture ofthe optimalso- lution . . . 12

1.3.2 Thealgebrai solution . . . 14

1.3.3 Theminimum-time algorithm . . . 19

1.3.4 Simulations results . . . 23

1.4 Minimum-time onstrained velo ityplanning . . . 24

1.4.1 Problemstatement andsu ient ondition . . . 25

1.4.2 An approximated solutionusing dis retization . . . 32

1.4.3 Thebise tion algorithm . . . 36

1.4.4 Simulations results . . . 37

2 Path generation and autonomousparking 41

2.1 Multi-optimization of

η ³

^{-splinesforautonomous parking . . . . 42}

2.1.1 Thesmooth parkingproblem . . . 43

2.1.2 Shapingpaths sequen ewith

η ³

^{-splines . . . . . . . . . 51}

2.1.3 Setting up themulti-optimization . . . 56

2.1.4 Simulation results . . . 60

2.2 Path generation for a tru kandtrailer vehi le . . . 65

2.2.1 Smoothfeedforward ontrolofthetru kandtrailervehi le 67 2.2.2 The

η ⁴

^{-splines . . . . . . . . . . . . . . . . . . . . . . . 72}

2.2.3 A pathplanning example . . . 84

3 Time-optimal dynami path inversion 93 3.1 Introdu tion to dynami inversion. . . 94

3.1.1 Input-output dynami path inversion . . . 95

3.2 Time-optimal dynami path inversion for an automati guided vehi le . . . 96

3.2.1 Kinemati modeland problemstatement . . . 96

3.2.2 Thedynami path inversionalgorithm . . . 101

3.2.3 Example . . . 103

4 Replanning methods for the traje tory tra king 107 4.1 Re ursive onvex replanning . . . 109

4.1.1 Traje torytra king for the uni y le . . . 109

4.1.2 Re ursivetra king in ageneral setting . . . 117

4.1.3 Appli ation to the tra king problem forthe uni y le . . 121

4.1.4 Simulation results . . . 124

4.1.5 Experimental results . . . 126

4.2 Iterativeoutput replanningfor at systems . . . 128

4.2.1 Problemstatement . . . 128

4.2.2 An Hermiteinterpolationproblem . . . 131

4.2.3 Iterative ontrol law . . . 135

4.2.4 Mainresults. . . 136

4.2.5 Simulation resultsfor the ase ofa uni y le . . . 139

4.2.6 Simulation resultsfor the ase ofa one-trailer system. . 144

Con lusions 149

Bibliography 151

A knowledgements 159

1 The overall ar hite ture for the optimal motion ontrol of the

wheeledvehi le. . . 2

1.1 Thesystemmodel for velo ity planning. . . 13

1.2 Anexampleoftheminimum-time ontrol(jerk)prole. . . 14

1.3 Theoptimalprolesofjerk

¯ u(t)

^,a eleration

¯ a(t)

^{,velo ity}

v(t) ¯

^,and

spa e

¯ s(t)

^{forexample1. . . . . . . . . . . . . . . . . . . . . . . . 24}

1.4 Theoptimalprolesofjerk

¯ u(t)

^,a eleration

¯ a(t)

^{,velo ity}

v(t) ¯

^,and

spa e

¯ s(t)

^{forexample2. . . . . . . . . . . . . . . . . . . . . . . . 24}

1.5 The pseudo-optimal proles of jerk

u(t) ¯

^, a eleration

¯ a(t)

^{, velo ity}

¯

v(t)

^{,andspa e}

¯ s(t)

^{forexample1. . . . . . . . . . . . . . . . . . . 38}

1.6 The pseudo-optimal proles of jerk

u(t) ¯

^, a eleration

¯ a(t)

^{, velo ity}

¯

v(t)

^{,andspa e}

¯ s(t)

^{forexample2. . . . . . . . . . . . . . . . . . . 39}

1.7 The pseudo-optimal proles of jerk

u(t) ¯

^, a eleration

¯ a(t)

^{, velo ity}

¯

v(t)

^{,andspa e}

¯ s(t)

^{forexample3. . . . . . . . . . . . . . . . . . . 40}

2.1 The ar-like vehi le onthe Cartesian plane. . . 44

2.2 Parking spa e

P

^{with ar}

A(q)

^{andobsta les}

B i

^,

i = 1, . . . , n

^{. . 46}

2.3 Thevehi lefrom

q _s

^to

q _g

^{with forward path}

Γ ⁺ ₁

^{or ba kward}

Γ ⁻ ₁

^{. 47}

2.4 Thetwo-pathssequen es

{Γ ⁺ 1 , Γ ⁻ ₂ }

^and

{Γ ⁻ 1 , Γ ⁺ ₂ }

^{fortheparking}

planning. . . 48

2.5 The three-paths sequen es

{Γ ⁺ 1 , Γ ⁻ ₂ , Γ ⁺ ₃ }

^and

{Γ ⁻ 1 , Γ ⁺ ₂ , Γ ⁻ ₃ }

^for

the parkingplanning.. . . 49

2.6 Optimalparkingwithtwo-splinemaneuver

{p ⁻ 1 , p ⁺ ₂ }

^inexample

1.. . . 62

2.7 Plots of urvature and urvature derivative as fun tions of the ar length along the entire optimal spline maneuver

{p ⁻ 1 , p ⁺ ₂ }

in example 1. . . 62

2.8 Optimal parking with three-spline maneuver

{p ⁺ 1 , p ⁻ ₂ , p ⁺ ₃ }

ⁱⁿ example 1.. . . 63

2.9 Plots of urvature and urvature derivative as fun tions of the ar lengthalongtheentireoptimalsplinemaneuver

{p ⁺ 1 , p ⁻ ₂ , p ⁺ ₃ }

in example 1. . . 63

2.10 Optimal parking with three-spline maneuver

{p ⁺ 1 , p ⁻ ₂ , p ⁺ ₃ }

ⁱⁿ example 2.. . . 64

2.11 Plots of urvature and urvature derivative as fun tions of the ar lengthalongtheentireoptimalsplinemaneuver

{p ⁺ 1 , p ⁻ ₂ , p ⁺ ₃ }

in example 2. . . 64

2.12 S hemati of atru kandtrailer vehi le. . . 67

2.13 Thepolynomial

G ⁴

-interpolating problem. . . 73

2.14 Symmetryof the

η ⁴

^{-spline. . . . . . . . . . . . . . . . . . . . . 82}

2.15 The

η ⁴

^-splinewith

¨ κ A

^varyingin

[−3, 3]

^{. . . . . . . . . . . . . 85}

2.16 The

η ⁴

^-splinewith

¨ κ _B

^varyingin

[−3, 3]

^{. . . . . . . . . . . . . 86}

2.17 The

η ⁴

^-splinewith

η ₁

^varyingin

[4, 25]

^{. . . . . . . . . . . . . . 86}

2.18 The

η ⁴

^-splinewith

η ₂

^varyingin

[4, 25]

^{. . . . . . . . . . . . . . 87}

2.19 Theoptimal steering

¯ δ(s)

^{for problem (2.84). . . . . . . . . . . 88}

2.20 Theoptimal maneuver paths forproblem (2.84). . . 88

2.21 Theoptimal steering

¯ δ(s)

^{for problem (2.86). . . . . . . . . . . 89}

2.22 Theoptimal maneuver paths forproblem (2.86). . . 90

2.23 Theoptimal steering

¯ δ(s)

^for multi-optimization problem(2.88) 91 2.24 Theoptimalmaneuverpathsformulti-optimizationproblem(2.88) 91 3.1 Dynami inversionbased ontrol. . . 94

3.2 Feedforward/feedba k ontrol s heme. . . 95

3.3 Awheeled AGVon aCartesian plane. . . 97

3.4 Theinterpolations onditions at the endpointsof path

Γ

^{.. . . . 100}

3.5 Geometri onstru tion ofthe forward path

Γ _f

^{. . . . . . . . . . 102}

3.6 Geometri alinterpretation ofequation system(3.14). . . 103

3.7 Theplanned path

Γ

^{and the asso iatedforward path}

Γ _f

^{of the} AGV. . . 105

3.8 Theoptimal velo ityprole

¯ v(t)

^{. . . . . . . . . . . . . . . . . . 106}

3.9 Theoptimal steering ontrol

δ(t) ¯

^{. . . . . . . . . . . . . . . . . . 106}

4.1 S hemati of auni y le mobilerobot. . . 110

4.2 Convex replanning. . . 113

4.3 The

C ³

-transition polynomial

λ(t)

^{. . . . . . . . . . . . . . . . . 114}

4.4 Re ursivegeneration of referen etraje tories. . . 115

4.5 Thehybridfeedforward/feedba ks hemeforthetraje torytra k- ingof wheeledmobilerobots. . . 115

4.6 a)The robottraje toryandb) the ontrol inputs forthe re ur- sive method. . . 125

4.7 a)Therobottraje toryandb)the ontrolinputsforthemethod presented bySamson. . . 125

4.8 a) Referen e and a tual traje tory for a ir le b) the norm of the

(x, y)

^{omponent of the tra king error with respe tto time. 127} 4.9 a)Referen e anda tualtraje toryfora ompositesplineb) the normofthe

(x, y)

^{omponent ofthe tra king errorwith respe t} to time. . . 127

4.10 The iterative replanning method. The gure shows the refer- en e output traje tory

y _d

^{, the a tual system output}

y

^{and the} replanned traje tory

y _p

^{. . . . . . . . . . . . . . . . . . . . . . . 132}

4.11 Theiterative ontrols hemefor thetraje torytra kingofaat system. . . 132

4.12 Therst three Hermitepolynomials for

r + l = 4

^and

T = 1

^{.. . 134}

4.13 Simulation resultsforuni y le systemwith theiterative replan-

ningmethod. . . 140

4.14 The ontrol inputs for the iterative replanning method applied

to the uni y le system. . . 141

4.15 Theerror fun tionsfor the iterative replanningmethodapplied

to the uni y le system. . . 141

4.16 Thereferen etraje tory

y _d

^{,thereplannedone}

y _p

^{andthea tual}

uni y le output

y

^{, forthe uni y leexample. . . . . . . . . . . . 142}

4.17 Simulation resultsfor the uni y le with the Samson's method. . 143

4.18 The ontrolinputs forthe Samson'smethod appliedto theuni-

y lesystem. . . 143

4.19 The error fun tions for the Samson's method applied to the

uni y le system.. . . 144

4.20 Tra king results for the one-trailer systemon aperiodi spline,

in the tru kpulling trailer ase. . . 145

4.21 The ontrol inputs for the iterative replanning method applied

to the one-trailer system,in the tru k pullingtrailer ase. . . . 146

4.22 a) The error fun tions for the iterative replanning method ap-

plied to the one-trailersystem, in the tru k pullingtrailer ase

and b)a lose upof iton asmallertime interval. . . 146

4.23 Tra king results for the one-trailer systemon aperiodi spline,

in the tru kpushing trailer ase. . . 147

4.24 The ontrol inputs for the iterative replanning method applied

to the one-trailer system,in the tru k pushingtrailer ase. . . . 147

4.25 a) The error fun tions for the iterative replanning method ap-

pliedto the one-trailersystem,in thetru kpushingtrailer ase

and b)a lose upof iton asmallertime interval. . . 148

2.1 Dimensionand stru ture ofthe sear hspa e

Z

^{. . . . . . . . . . 57}

Thisthesispresentssomeresultsobtained duringmyPhD ourseDottoratoin

Te nologie dell'Informazione, at theUniversità diParma, Dipartimento di In-

gegneria dell'Informazione, in the threeyears period2009-2012.The workhas

beenfo usedontheproblemofthetime-optimalmotion ontrolofwheeledau-

tonomoussystems,su hasuni y lerobots,automati guidedvehi les(AGVs),

ar-like vehi les andtru kand trailer(or one-trailer) systems.

The aim is to obtain a ontrol that provides a smooth motion of the un-

manned vehi lein minimum-time. Inorder to do that, it is ne essaryto plan

a path with anappropriate geometri ontinuity, and two time-optimal input

signals of velo ity and steering angle ontinuous with their derivatives. More-

over,afeedba k ontroller mustbeadoptedtoguaranteetherobustnessofthe

overall ontrols heme. Finalresultofthethesis anbeviewedasthesynthesis

of various methods for hybrid feedforward/feedba k ontrol for a wide lass

of wheeled mobile robots. Figure 1 presents a on eptual s heme that sum-

marizes theideabehindthehybridfeedforward/feedba k ontrol,whi histhe

nalresultof the work done alongthe three yearsof study andresear h.

Pathplanningandvelo ityplanning anbe ompletelyindependenttoea h

other, on onditionthat:

1) the planned path has an appropriate geometri ontinuity and satises

geometri interpolating onditions at the path endpoints, and

2) the velo ity is a

C ¹

^{-fun tion satisfying} interpolating onditions (on dis- tan e, velo ityand a elerations) at the endpoints of the planned time-

interval.

Velocity planning

Path planning

Unmanned vehicle

Trajectory Tracking

Feedforward control

Feedback control Dynamic

path inversion

Figure1:Theoverallar hite turefortheoptimalmotion ontrolofthewheeled

vehi le.

Indeed,givenasu ientlysmoothpath,adynami inversionpro edure anbe

appliedto determinethefeedforward ontrolinputsoftheautonomousvehi le

still maintaining freedom in the planning ofvelo ityinput.

Hen e, the thesis rst shows some methods that permit to plan a path

andoptimal inputsignalswhi hleadto aminimum-time smoothmotion for a

varietyofautomati guidedsystemsinnominal onditions(i.e.nonoiseae ts

the systems).Se ondly, itisshown howguarantee thetra king ofthe planned

traje tory by means of a feedba k ontrol, when the system is ae ted by

additivenoise.

Theveryrstpartof thethesis( hapter1)fa esthe time-optimalvelo ity

planning with arbitraryboundary onditions for anautomati guidedvehi le.

Initially, only a onstraint on the maximum value of the jerk (i.e. the velo -

ity se ond derivative) is onsidered. The addressed minimum-time planning

problemhasbeen re astinto aninput- onstrained minimum-time rea hability

ontrol problemwith respe tto a suitable state-spa esystem, where the on-

trol inputisa tuallythe sought jerkofthe velo ityplanning.Byvirtueofthe

well-known Pontryagin's Maximum Prin iple the optimal input- onstrained

ontrol is then a bang-bang fun tion. An algebrai approa h to obtain this

optimal solutionhasbeen devisedand a newalgorithm to ompute the bang-

bang jerk prole is exposed. This problem has been re onsidered introdu ing

onstraints also on the maximum values of the velo ity and a eleration. In

this ase the Pontryagin's Maximum Prin iple does not ensure the existen e

of the time-optimal ontrol. Su ient onditions, guaranteeing the existen e

ofasolutiontothe minimum-time onstrainedplanningproblem,areexposed.

Thetime-optimal ontrol isnot a lassi bang-bangfun tion,but itshallbe a

generalizedbang-bang.Theproblemhasbeenfa edthroughdis retizationand

the obtained solutionisbased ona sequen eoflinearprogramming feasibility

he ks,dependingon motion onstraints andboundary onditions.

Chapter 2 presents two methods for the path planning of ar-like andone

trailer vehi les. It is shown how plan paths with an appropriate geometri

ontinuity byresolving ageometri interpolation. In parti ular,the geometri

interpolation problem, whi h has innite dimension, has been re ast into a

polynomialinterpolationproblem(anitedimensionproblem),bymeansofthe

η

^{-splines.Theshapingofthiskindofsplinedependsonave torofparameters}

alled eta, and on the boundary onditions. It is then presented a multi-

optimization pro ess to optimally hoose these free parameters, with the aim

to plantraje torythatrespe tboundson urvature and urvaturederivative,

ensuringavoidan e ofthe obsta les inthe real workspa e.Inthe ase ofthe

ar-likevehi le,appli ationstotheautonomousparkingproblemarepresented.

In hapter 3,thedynami pathinversionblo k ( f.gure1)is outlinedby

introdu ing apro edurethatpermitsto obtain aminimum-time steering on-

trol input for an automati guided vehi le (AGV). One an onsider to have

just planned a path and a time-optimal velo ity prole exploiting the te h-

niques introdu ed in the rst two hapter. The optimal steering input signal

for theAGVisobtained witha dynami inversiononthe plannedpath,based

onsomegeometri propertiesofthepathitself,andoftheAGVkinemati sys-

tem.Similar pro edure anbeeasilydetermined fortheothervehi les,su has

the ar-like and the one-trailer.

Finally, hapter4proposestwo methods forthe traje torytra king forau-

tonomous systems ae ted by additive noise. Both methods are thought for

ases where ontinuous-time or high-frequen y revelation of the systemstate

or output is not possible or not e onomi al and only low-frequen y feedba k

ispra ti able. Theimplemented solutions tothis traje torytra kingproblem,

relies on iterative replanningmethods to ompute a newreferen e traje tory,

used to generate the feedforward inverse ommand velo ities that help in re-

du ingthetra kingerrors.Forbothte hniquesexpli it losed-formboundson

the tra kingerror areprovided.

Minimum-time velo ity

planning

Plansareonlygoodintentionsunless

they immediately degenerateintohardwork

PeterDru ker

In the wide eld of vehi les autonomous navigation, signi ant resear h

eorts have been dedi ated to the problem of optimal motion planning. The

problem of motion planning for autonomous guided vehi les is a well known

and studied issue in roboti s, see for example the re ent books [1℄ and [2℄.

This hapterproposete hniques for minimum-time velo ityplanning with ar-

bitrary boundary onditions, onsidering two dierent ases: one with only

onstraint onthe maximum absolute value of the jerk(i.e the velo ity se ond

derivative), and one with onstraints also on the maximum absolute value of

the a eleration and velo ity. The minimum-time velo ity planning is ast in

the ontextof theso- alled path-velo ityde omposition [3℄usingthe iterative

steering navigation te hnique [4,5℄.

The rst two se tions briey introdu e the optimal ontrol theory, with

parti ular attention to the linear time-optimal problem. For more details on

this arguments see, for example,books [6,7℄.

The third se tion presents a pro edure for the synthesis of a velo ity

C ¹

^-

fun tionthatpermitsinminimum-time andwithaboundedjerktointerpolate

givenvelo ityand a elerationat the time planningintervalendpointsandto

travel a given distan e. The onditionon the maximum jerkvalue permits to

obtain a smooth velo ity prole [8℄. The addressed minimum-time planning

problem will be re ast into an input- onstrained minimum-time rea hability

ontrolproblemwithrespe ttoasuitablestate-spa esystem,wherethe ontrol

inputisa tuallythesought jerkofthevelo ityplanning.Byvirtueofthewell-

knownPontryagin'sMaximumPrin ipletheoptimalinput- onstrained ontrol

isthen abang-bang fun tion.

Finally, a solution for the onstrained minimum-time velo ity planning is

presented. In this ase, the time-optimal solution is not a lassi bang-bang

fun tion, but it shallbe a generalized bang-bang fun tion [9℄.The minimum-

time transitionisobtained bydis retizing the ontinuous-time model andfor-

mulating an equivalent dis rete-time optimization problem solved by means

of linear programming te hniques. More pre isely, boundary onditions and

problem onstraints are expressed by linear inequalities on a olumn ve tor

u

^, representing the input signal (i.e the jerk) at sampling times. Hen e, the minimum-timeplanningproblemisreformulatedasafeasibilitytestforalinear

programmingproblem,andtheminimumnumberofstepsrequiredto omplete

thegiventransition anbefoundthroughasimplebise tionalgorithm.Theuse

of linearprogramming te hniques for solvingminimum-time problems for lin-

eardis rete-timesystems subje ttobounded inputsdatesba kto Zadeh[10℄.

Subsequently, many ontributions haveappearedfo using onvarious improve-

ments. For example a faster algorithm is proposedin [11℄.For what on erns

time-optimal ontrol for ontinuous-time systems, a related result, under dif-

ferent hypotheses, ispresented in [12℄.

1.1 Optimal ontrol theory

Optimal ontrol isthe pro essofdetermining ontrolandstatetraje toriesfor

a dynami system over a period of time, in order to minimize a performan e

index. Themethod is losely relatedin its originsto the theoryof al ulus of

variations and it islargely due to the work of Ri hard Bellman [13℄, and Lev

Pontryaginet al. [14℄.Optimal ontroland its rami ationshave foundappli-

ationsin manydierent elds,in luding aerospa e,pro ess ontrol, roboti s,

bioengineering,e onomi s,andit ontinuestobeana tiveresear hareawithin

ontrol theory.

1.1.1 Problem statement and notation

Consider optimal problems dened by the onstraint set

C

^{, a subset of the}

tangent bundleofa smooth manifold

M

^{,and a ost fun tion}

f

^{, thatisareal-}

valued fun tion having

C

^{as its domain. A traje tory of}

C

^{is an absolutely}

ontinuous urve

x(t) ∈ M

^{su h that}

^dx _dt (t) ∈ C

^{for almostall}

t

^{in the domain}

of

x

^{. Thetotal ost of}

x

^{isdened as}

Z _T

0

f  dx dt (t)

 dt ,

where

[0, T ]

^{denotes the domain of}

x

^{. Given any two points}

x ₀

^and

x _f

ⁱⁿ

M

^,

theoptimaltraje tory of

C

^{isthe onewhi h onne ts}

x ₀

^to

x _f

^{andwhosetotal}

ost isminimalamong allsu htraje tories of

C

^.

The onsidered sets

C

^{admit se tionsof the form}

ξ = F (π(ξ), u ₁ , . . . , u _m )

^,

where

(u ₁ , . . . , u _m )

^{takesvaluesin axedset}

U ∈ R ^m

^,

π

^{indi atesthe natural}

proje tion from

T M

^onto

M

^{, and}

ξ

^{is an arbitrary point of}

C

^{. Then, the}

traje toryvelo ity

dx

dt

^isparametrizedbythe ontrols

u ₁ , . . . , u _m

^{, anditstotal}

ost anbe expressedas

Z _T

0

c(x(t), u(t))dt = Z _T

0 f ◦ F (x(t), u(t))dt .

In a given se tion of

C

^{, the} traje tories of

C

^{that onne ts two given points}

x 0

^and

x _f

^{in a nite time}

T

^{, oin ide with the solution urves}

x(t)

^{of the}

dierential system



 

 

dx

dt = F (x(t), u(t), . . . , u m (t)) x(0) = x ₀

x(T ) = x _f .

Undersuitablesmoothnessassumptionson

F

^{,ea h ontrolfun tion}

u(t)

^deter-

mineauniquesolution urve,sotheproblemofndingtheoptimaltraje tories

of

C

^{is onverted to one of nding the ontrols that give rise to the optimal}

traje toryand thatisan optimal ontrol problem.

Weshallneedadditionalnotation.Foranymatrix

C

^,

C ^′

^{indi atesitstrans-}

pose,while

span(C)

^{representsthesetofallthe}eigenvaluesof

C

^{.Foranyve tor}

spa e

E

^{, its dual isdenoted by}

E ^∗

^.

E

^{an be regarded as a subspa e of}

(E ^∗ ) ^∗

through the orresponden e

e → g(e)

^{for any}

e ∈ E

^and

g ∈ E ^∗

^{. When}

E

^is

nite-dimensional,

E = (E ^∗ ) ^∗

^{. Re all that a linear mapping}

L : E → E ^∗

^is

said to be symmetri if

L

^{is equal to itsdualmapping}

L ^∗

^.

1.2 Linear time-optimal problem

Thepro essoftransferringonestate intoanotheralongatraje toryofagiven

dierential system su h that the time of transfer is minimal is known as the

minimal-time problem, and it is one of the basi on erns of optimal ontrol

theory. Consider the lineartime-invariant system,

dx

dt = Ax + Bu ,

^(1.1)

with

x ∈ M ⊂ R ⁿ

^and

u ∈ U c ⊂ R ^m

^{, where}

A

^and

B

^{are onstant matri esof}

order

n ×n

^and

n ×m

respe tively.Letsystem(1.1)bedenedinareal,nite- dimensional ve tor spa e

M

^{in whi h the ontrol fun tions are restri ted to}

a ompa t and onvex neighborhood

U _c

^{of the origin, in a} nite-dimensional ontrol spa e

U

^{, and also assume that (1.1) is} ontrollable and that ontrol fun tions are measurable. A traje toryis dened by the pair

(x, u)

^{, in whi h}

x

^{is an absolutely ontinuous urve of some time interval}

[0, T ]

^,

T > 0

^{, that}

satises(1.1) almosteverywherein

[0, T ]

^.

Denition 1 Atraje tory

(x, u)

^{is alled} time-optimalon an interval

[0, T ]

^if

for any other traje tory

(y, v)

^{of (1.1) dened on its interval}

[0, S]

^{, for whi h}

y(0) = x(0)

^and

y(S) = x(T )

^,

S

^{islarger than or equal to}

T

^.

Theorem 1 For any time-optimal traje tory

(x, u)

^{on an interval}

[0, T ]

a) the terminal point

x(T )

^{belongs to the boundary}

∂A(x(0), T )

^{of the set of}

rea hable points from

x(0)

^at

t = T

^{of system (1.1);}

b) anypoint

b

^{that belongs totheboundary ofthesetrea hablefromtheorigin}

attime

T

^{istheterminalpointofa}time-optimaltraje toryontheinterval

[0, T ]

^.

Proof. If

x(T )

^{belonged to the interior of}

A(x(0), T )

^{, then}

x(T )

^{would also}

belongto the interior of

A(x(0), T − ǫ)

^{, for some}

ǫ > 0

^{, whi h is not possible,}

be ause that would violate the time optimality of

(x, u)

^{on the} time-interval

[0, T ]

^{. This argument proves parta).}

To prove b), notethat for any

T > 0

^{, points on the boundary of}

A(0, T )

annotberea hedinatimeshorterthan

T

^{.Ontheotherhand}

A(0, T )

^{is om-}

pa tfor ea h

T > 0

^{. Therefor,for ea h}

b

^on

∂A(0, T )

^{thereexistsatraje tory}

(x, u)

^{dened on the} time-interval

[0, T ]

^{su h that}

x(0) = 0

^and

x(T ) = b

^{. It}

follows bythe foregoing argument that

(x, u)

^is time-optimalon

[0, T ]

^.



1.2.1 The maximum prin iple

For the minimum-time ontrol problems, the Pontryagin maximum prin iple

providesthe ne essaryandthe su ient onditions for optimality. The reader

is re ommended to onsult [6, pp. 305306℄ for the proof of the theorem and

other details.

Theorem 2 (Pontryagin's Maximum Prin iple) Anytime-optimaltraje -

tory

(¯ x, ¯ u)

^{on aninterval}

[0, T ]

^{istheproje tion ofanintegral urve}

(¯ x, ¯ p, ¯ u)

^of

theHamiltonianve toreld

H ~

^{asso iated with}

H(x, p, u) = −p 0 + p(Ax + Bu)

^,

with

p 0

^{equal to either}

0

^or

1

^{, su h that}

a)

H(¯ x(t), ¯ p(t), ¯ u(t)) = max _{u∈U c} H(¯ x(t), ¯ p(t), u)

^{for almost all}

t

ⁱⁿ

[0, T ]

^;

b)

H(¯ x(t), ¯ p(t), ¯ u(t)) = 0

^{almost everywhere in}

[0, T ]

^;

)

p(t) 6= 0 ¯

^{for any}

t

^{, if}

p ₀ = 0

^.

Proof. See[6, pp. 305306℄.



Remark Thefollowingremarksarehelpfulfor larifysomeimportantaspe ts

and onsequen esof themaximum prin iple:

1.

H

^{should be regarded as a fun tion on}

T ^∗ M = M × M ^∗

parametrized by both the hoi eof a ontrol fun tion and the value of

p ₀

^.

2. Assumethat

u(t)

^{isagivenmeasurable ontrolfun tionwithvaluesin}

U _c

^.

Ea hintegral urve

σ(t) = (x(t), p(t))

^{of the} Hamiltonianve tor eld

H ~

asso iatedwith

H(x, p, u(t)) = −p 0 + p(Ax + Bu(t))

^{, whenexpressed in}

anoni al oordinates,satisesthefollowingpairofdierentialequations:

dx

dt = Ax(t) + Bu(t) , dp

dt = −A ^∗ p(t) .

3. The maximality ondition a) of theorem 2 is equivalent to

p(t)B ¯ ¯ u(t) = max _{u∈U c} p(t)Bu ¯

^{for almostall}

t

ⁱⁿ

[0, T ]

^.

1.2.2 Bang-bang prin iple for s alar systems

The bang-bang prin iple says that the optimal ontrols take the most advan-

tage ofpossible ontrol a tion at ea h instant. Thename is motivatedbythe

parti ular ase of a ontrol spa e given by the interval

U _c = [u ⁻ , u ⁺ ]

^{, where}

optimal ontrolsmustswit hbetweentheminimalandmaximalvalues

u ⁻

^and

u ⁺

^{. There are various theorems that make this prin iple rigorous. Here, the}

simplestone is reported, asSontag statedin [7, pp. 436437℄.

Theorem 3 (Weak bang-bang) Assumethat thematrixpair

(A, B)

^{is on-}

trollable. Let

u ¯

^{be a ontrol steeringsystem (1.1) from an initial state}

x ₀

^{to a}

nal state

x _f

^{in minimal time}

T > 0

^{. Then,}

u ∈ ∂U ¯ ^c

^{for almost}

t

ⁱⁿ

[0, T ]

^.

Proof. Theproofdire tlyderivesfromtheappli ationofthePontryagin'smax-

imum prin iple (see[7, pp. 436437℄).



Thanks to theorem 2 it is possible to state that the time-optimal ontrol

¯

u

^{isunique and itisalso possibledetermine its stru ture(for amore rigorous}

treatment see[7℄and [15℄).

We spe ialize now to singleinput systems(

m = 1

^{),and write}

b

^{instead of}

B

^{in (1.1).Ingeneral}

U _c = [u ⁻ , u ⁺ ]

^{, but we willtake,in order to simplifythe}

exposition,

u ⁻ = −1

^and

u ⁺ = 1

^{. Assumethat the pair}

(A, b)

^is ontrollable.

Forea htwostates

x ₀

^and

x _f

^{,thereisaunique}time-optimal ontrol

u ¯

^steering

x ₀

^to

x _f

^{, and thereis a nonzerove tor}

γ ∈ R ⁿ

^{su h that}

¯

u(t) = sgn(γ ^′ e ^{− tA} b) ,

^(1.2)

for all

t / ∈ S γ,T

^{, where}

S γ,T = t ∈ [0, T ] : γ ^′ e ^−tA b = 0 ,

is a nite set. This means that the optimal ontrol

u ¯

^{is a pie ewise onstant}

fun tion, whi h swit hes between values

−1

^and

1

^{. The following} proposition permitstodetermine the number ofswit hingsin the aseof systemmatrix

A

hasonly realeigenvalues.

Proposition 1 Suppose that the matrix

A

^{has only}

n

^real eigenvalues,i.e.

span(A) ∈ R .

Then,for ea h

γ

^,

b

^and

T

^,

S _γ,T

^{as atmost}

n − 1

^{elements,wherebyany time-}

optimal ontrol for system (1.1) as no more than

n − 1

swit hings.

Proof. This proposition derives dire tly from the appli ation of the Pontrya-

gin'smaximumprin ipletothetime-optimal ontrolofas alarsystem.Reader

an ndseveral proofsofthis proposition (see,for example[7,15℄).

1.3 Minimum-time velo ity planning with arbitrary

boundary onditions

This se tion introdu es and explains the approa h presented in [16℄, whi h

solves the minimum-time velo ity planning problem with arbitrary boundary

onditionsanda onstraintonthemaximumjerkvalue.Theobtainedoptimal-

timesolution,basedonPontryagin'sMaximumPrin iple,isasmoothplanning

with ontinuousvelo ities and a elerations. The devised algebrai algorithm

tosolvethisminimum-timeplanningproblemiswellsuited tobeimplemented

within the framework ofpath-velo ity de omposition for autonomousnaviga-

tion.

1.3.1 Problem statement and the stru ture of the optimal so-

lution

The following denition willbeused alongthis paper.

Denition 2 A fun tion

f : R → R, t → f (t)

^{has a}

P C ²

ontinuity, and we write

f (t) ∈ P C ²

^if

a)

f (t) ∈ C ¹ (R) ,

b)

f (t) ∈ C ² (R − {t 1 , t ₂ , . . . }) ,

)

∃ lim _t→t ⁻ _i D ² f (t) , ∃ lim _t→t ⁺ _i D ² f (t) , i = 1, 2, . . .

where

{t 1 , t ₂ , . . . }

^{is a set of} dis ontinuity instants.

The problem is to plan a minimum-time smooth velo ity prole

v(t) ∈ P C ²

while a given onstraint on the maximum jerk value

j M

^{is guaranteed and}

the initial and nal onditions onthe velo ity anda eleration arearbitrarily

assigned. Formally:

v∈P C min ² t _f ,

^(1.3)

su h that

Z _{t f}

0

v(ξ)dξ = s _f ,

^(1.4)

v(0) = v ₀ , v(t _f ) = v _f ,

^(1.5)

˙v(0) = a ₀ , ˙v(t _f ) = a _f ,

^(1.6)

|¨v(t)| ≤ j M , ∀t ∈ [0, t f ] ,

^(1.7)

where

s _f > 0

^,

j _M > 0

^and

v ₀ , v _f , a ₀ , a _f ∈ R

^{are arbitrary velo ity and}

a eleration boundary onditions.

s _f

^{is the total length of the path and}

t _f

^is

the travelling time to omplete this path. The solution of the above problem

is

v(t) ∈ P C ¯ ²

^{with asso iated}minimum-time

¯ t _f

^.

Theminimum-time planningproblem(1.3)-(1.7) an beeasily re astto an

input- onstrained minimum-time ontrol problem with respe t to a suitable

state-spa esystem.Indeed onsiderthejerk

¨ v(t)

^{asthe ontrolinput}

u(t)

^ofa

as ade ofthree integrators asdepi ted in gure1.1.

PSfragrepla ements

1 s 1

s 1

s

u(t) ˙v(t) v(t) s(t)

Figure1.1: Thesystemmodelfor velo ityplanning.

Introdu ing the state

x(t)

^{asthe olumn ve tor}





 x ₁ (t) x ₂ (t) x 3 (t)





 :=





 s(t) v(t)

˙v(t)





 ,

the systemisrepresented bythe dierential equation

x(t) = A x(t) + B u(t) = ˙







0 1 0 0 0 1 0 0 0





 x(t) +





 0 0 1





 u(t) .

^(1.8)

Hen e, problem (1.3)-(1.7) is equivalent to nd a time-optimal ontrol

u(t) ¯

thatbrings system(1.8)fromtheinitialstate

x(0) = [0 v 0 a 0 ] ^′

^{tothenalstate}

x(¯ t _f ) = [s _f v _f a _f ] ^′

^{in minimum time}

¯ t _f

^{, while satisfyingthe input onstraint}

|¯u(t)| ≤ j M , ∀t ∈ [0, ¯t f ] .

Inse tions 1.2.1and1.2.2ithasbeenexposedthatthe Pontryagin's maxi-

mum prin iple givesane essaryandsu ient onditionforthis lassofprob-

lems. Moreover, it has been shown that in the ase of a linear s alar system

the time-optimal ontrol

u(t) ¯

^{is abang-bang fun tion.In our aseit willbe a}

pie ewise onstantfun tion thatswit hesbetweenthe

−j M

^and

+j _M

^{. Finally,}

another information on the optimal ontrol stru ture is obtained frompropo-

sition 1. Considering that system(1.8) has three null eigenvalues we dedu e,

by virtue of proposition 1, that the time-optimal jerk

u(t) ¯

^{has at most two}

swit hinginstants.Hen e, thegeneralstru tureofthe optimal

u(t) ¯

^{isdepi ted}

in gure1.2where

u _M ∈ {−j M , +j _M }

^and

0 ≤ t 1 ≤ t 2 ≤ ¯t f

^with

t ¯ _f > 0

^.

PSfragrepla ements

¯ u(t)

u _M

−u ^M

t ₁ t ₂ ¯ t _f t

Figure1.2: Anexampleoftheminimum-time ontrol(jerk)prole.

1.3.2 The algebrai solution

It has been shown above the stru ture of the time-optimal ontrol

u(t) ¯

^{. In}

the following,analgebrai approa h willbeexposedto exa tlydetermine this

optimal jerkprole.

Exploiting the boundary onditions (1.3)-(1.6), the problemis to nd the

swit hingtime values

t 1

^and

t 2

^{, the minimum time}

¯ t _f

^{andthe signof the jerk}

initialvalue

u(0) ¯

^{,whilesatisfyingthe onstraint}

0 ≤ t 1 ≤ t 2 ≤ ¯t f

^with

¯ t _f >

0

^{. From the boundary ondition (1.6) on the nal}a eleration value we know that

a ₀ + Z ¯ t f

0

¯

u(ξ)dξ = a _f .

Integratingtheoptimaljerkproleonthethreeintervals,thefollowingrelation

isobtained

a ₀ + Z t 1

0

u _M dξ + Z t 2

t 1

(−u M )dξ + Z ¯ t f

t 2

u _M dξ = a _f ,

and nallya rstlinearequation in

t ₁

^,

t ₂

^and

¯ t _f

^{is found}

2 u _M t ₁ − 2 u M t ₂ + u _M t ¯ _f = a _f − a 0 .

^(1.9)

The a eleration prole

x ₃ (t)

^{is obtained by} integrating the optimal jerk a - ording to

x ₃ (t) = a ₀ + Z _t

0

¯

u(ξ)dξ , ∀t ∈ [0, ¯t f ] ,

thatresults in the following equation

x ₃ (t) =



 

 

a 0 + u M t t ∈ [0, t ¹ ]

a ₀ + 2 u _M t ₁ − u M t t ∈ [t 1 , t ₂ ] a ₀ + 2 u _M t ₁ − 2 u M t ₂ + u _M t t ∈ [t 2 , ¯ t _f ] .

(1.10)

Now, by virtue of the boundary ondition (1.5), the following relation is de-

du ed

v ₀ + Z ¯ t f

0

x ₃ (ξ)dξ = v _f ,

hen e, from(1.10), one obtains

v 0 + Z t 1

0

(a 0 + u M ξ)dξ + Z t 2

t 1

(a 0 + 2 u M t 1 − u ^M ξ)dξ +

Z ¯ t f

t 2

(a ₀ + 2 u _M t ₁ − 2 u M t ₂ + u _M ξ)dξ = v _f .

Finally, aquadrati equationin

t ₁

^,

t ₂

^and

t ¯ _f

^isfound

−u M t ² ₁ + 2 u _M t ₁ ¯ t _f + u _M t ² ₂ − 2 u M t ₂ ¯ t _f + 1

2 u _M ¯ t ² _f + a ₀ t ¯ _f = v _f − v 0 .

^(1.11)

Integrating the a eleration fun tion

x ₃ (t)

^asfollows

x ₂ (t) = v ₀ +

Z t 0

x ₃ (ξ)dξ , ∀t ∈ [0, ¯t f ] ,

the velo ityprole

x 2 (t)

^{is obtained}

x 2 (t) =



 

 

 

 

 

 

 

 

 

 

v ₀ + a ₀ t + ¹ ₂ u _M t ² t ∈ [0, t 1 ] v ₀ + a ₀ t + 2 u _M t ₁ t − u M t ² ₁ − ¹ ₂ u _M t ² t ∈ [t 1 , t ₂ ]

1

2 u _M t ² − u M t ² ₁ + u _M t ² ₂ + 2 u _M t ₁ t

−2 u M t ₂ t + a ₀ t + v ₀ t ∈ [t 2 , ¯ t _f ] .

(1.12)

By virtueof the boundary ondition(1.4), the following relation holds

Z ¯ t f

0

x ₂ (ξ)dξ = s _f ,

then, from(1.12), we dedu e

Z _{t 1}

0

(v ₀ + a ₀ ξ + 1

2 u _M ξ ² )dξ + Z _{t 2}

t 1

(v ₀ + a ₀ ξ + 2 u _M t ₁ ξ − u M t ² ₁

− 1

2 u _M ξ ² )dξ + Z ¯ t f

t 2

( 1

2 u _M ξ ² − u M t ² ₁ + u _M t ² ₂ + 2 u _M t ₁ ξ

−2 u M t ₂ ξ + a ₀ ξ + v ₀ )dξ = s _f .

Finally, the last ubi equationin

t ₁

^,

t ₂

^and

t ¯ _f

^{isgiven by}

1

3 u _M t ³ ₁ − u M t ² ₁ ¯ t _f + u _M t ₁ ¯ t ² _f − 1

3 u _M t ³ ₂ + u _M t ² ₂ ¯ t _f − u M t ₂ t ¯ ² _f + 1

6 u _M t ¯ ³ _f + 1

2 a ₀ ¯ t ² _f + v ₀ ¯ t _f = s _f .

(1.13)

Thetime-optimalvelo ityproleisplannedbysolvingthe nonlinearalgebrai

systemgiven byequations (1.9),(1.11) and (1.13).

Here, we onsider the ase of positive initial jerk (i.e.

u _M = +j _M

^{). From}

equation (1.9)follows

t 1 = t 2 − 1 2 ¯ t ² _f + 1

2

a _f − a 0

j _M .

^(1.14)

By substitutingrelation (1.14) in (1.11) the relationbelow holds

t 2 = h 3

4 j _M ¯ t ² _f − ¹ ₂ (3 a _f − a 0 ) ¯ t _f + _{4 j} ¹

M (a _f − a 0 ) ² + v _f − v 0

i

j _M t ¯ _f − a f + a ₀ .

^(1.15)

Bysubstitutionof(1.14)and(1.15)in(1.13),aquarti equationin

t ¯ _f

^unknown

isobtained

1

32 u ² _M t ⁴ ₃ + 1

8 u _M (a ₀ − a f ) t ³ ₃ +  1

2 u _M (v ₀ + v _f ) − 1

16 (a ² ₀ + a ² _f ) − 3 8 a ₀ a _f

 t ² ₃

+ 1

8 a 0 a _f

u _M (a ₀ − a f ) − 1 24

a ³ ₀ − a ³ f

u _M + a ₀ v _f − a f v ₀ − u M s _f

! t ₃ − 1

96

a ⁴ ₀ + a ⁴ _f u ² _M + 1

24 a ₀ a _f

u ² _M (a ² ₀ + a ² _f ) − 1 16

a ² ₀ a ² _f u ² _M − 1

2 (v ² ₀ + v _f ² ) + v ₀ v _f − a 0 s _f + a _f s _f = 0 .

(1.16)

Inthe aseofnegativeinitialjerk(i.e.

u M = −j ^M

^{),theoptimalsolution anbe}

foundby hangingthesignof

j _M

^{in(1.9),(1.11) and(1.13)andthen applying}

the same pro edure exposed above. In sake of simpli ity the three equations

systemfor this aseisomitted.

The optimal degenerate ase

Consider apositive initial jerkvalue(i.e.

u _M = +j _M

^{).A solutionof the three}

equations system (1.9), (1.11) and (1.13) existsonly if the following relation

holds (see(1.15))

j _M ¯ t _f − a f + a ₀ 6= 0 .

^(1.17)

If(1.17) is not veried,followsthat

a ₀ + j _M ¯ t _f = a _f ,

whi h orrespondsto the optimaldegenerate solutionexpressed by

t ₁ = t ₂ = 0 , ¯ t _f = a _f − a ⁰

j _M .

^(1.18)

Hen e, by virtueof ondition

¯ t _f > 0

^{thefollowing inequality musthold}

a _f > a ₀ .

The optimaldegenerate jerkis

¯

u(t) = j _M , ∀t ∈ [0, ¯t f ] .

^(1.19)

Notethatsolution(1.18)satisesequation(1.9).Integrating(1.19)onededu es

the a eleration fun tion

x ₃ (t) = a ₀ + j _M t , ∀t ∈ [0, ¯t f ] .

Inthe samewaythe optimal velo ityfun tion isobtained

x ₂ (t) = v ₀ + Z t

0

x ₃ (ξ)dξ = v ₀ + a ₀ t + 1

2 j _M t ² , ∀t ∈ [0, ¯t f ] ,

and then the optimal spa efun tion isgivenby

x ₁ (t) = Z _t

0

x ₂ (ξ)dξ = v ₀ t + 1

2 a ₀ t ² + 1

6 j _M t ³ , ∀t ∈ [0, ¯t f ] .

If

t = ¯ t _f

^{, byvirtueof the boundary onditions (1.3) and(1.4) follows that}

v ₀ + a ₀ ¯ t _f + 1

2 j _M ¯ t ² _f = v _f ,

^(1.20)

and

v ₀ ¯ t _f + 1

2 a ₀ ¯ t ² _f + 1

6 j _M ¯ t ³ _f = s _f .

^(1.21)

By substitutingrelation (1.18) in (1.20) the relationbelow isdedu ed

1 2

a ² _f − a ² 0

j _M + v ₀ − v f = 0 .

^(1.22)

Then, bysubstitutingrelation (1.18) in (1.21) the following equationholds

1 6

a ² _f j _M ² − 2

3 a ³ ₀ j ² _M − 1

2 a ² ₀ a _f

j _M ² + v ₀ a ₀

j M − v ₀ a _f

j M − s f = 0 .

^(1.23)

Relations(1.22) and(1.23)mustbesatisedinthe degenerate ase.Notethat

theyareexa tlythese ondandthethirdequationofsystem(1.9),(1.11),(1.13)

when ithassolution(1.18).

In ase of initial negative jerk (i.e.

u _M = −j M

^{), the optimal degenerate}

solution is

u(t) = −j ¯ M , ∀t ∈ [0, ¯t f ] ,

orrespondingto

t ₁ = t ₂ = 0 , ¯ t _f = a ₀ − a f

j _M .

^(1.24)

Thisdegenerate aseemerges with

a ₀ > a _f ,

and the following relations hold

1 2

a ² ₀ − a ² f

j _M + v ₀ − v f = 0 ,

^(1.25)

and

1 6

a ² _f j _M ² − 2

3 a ³ ₀ j ² _M − 1

2 a ² ₀ a _f

j _M ² − v ₀ a ₀

j _M + v 0 a _f

j _M − s f = 0 .

^(1.26)

1.3.3 The minimum-timealgorithm

The Minimum-Time Velo ity Planning (MTVP) algorithm is presented by

exploiting the algebrai solution exposed in subse tion 1.3.2. This algorithm

must veries if a positive or a negative jerk degenerate solution exists; after

that, ifa degenerate solutionwas not found it he ks the generi ases ofini-

tial positive andnegativejerk solutions.Hen e, the MTVPalgorithm an be

synthesizedasfollows:

begin

if a _f > a ₀ then procedure PJDS;

end

if a _f < a ₀ then procedure NJDS;

end

procedure PJS;

procedure NJS;

end

Then, the MTVP algorithm is omposed of four separated pro edures: the

PositiveJerk Degenerate Solution(PJDS), the Negative Jerk DegenerateSo-

lution (NJDS),thePositiveJerkSolution (PJS)andtheNegativeJerk Solu-

tion (NJS).Letus des ribe thesepro edures in detail.

Pro edurePJDS

This pro edure starts if

a _f > a ₀

^{, be ause is not possible to have a degener-}

ate solution with positive initial jerk (i.e.

u M = +j M

^{) if}

a _f ≤ a ⁰

^{. If ondi-}

tions (1.22)and (1.23) areveriedthe positivejerk degeneratesolution(1.18)

isimposedandthe MTVP algorithm isstopped, otherwisethe algorithm ex-

e ution returnsto the main program.The pro edureis asfollows:

begin if ¹ ₂ ^a

2 f −a ² ₀

j M + v ₀ − v f = 0 and

1 6

a ² _f

j _M ² − ² _{3 j} ^a 2 ^{3 0}

M − ¹ ₂ ^a _j ^{2 0} 2 ^{a f} M

+ ^{v 0} _j ^{a 0}

M − ^{v 0} _{j M} ^{a f} − s f = 0 then

[t 1 , t 2 , ¯ t _f ] = [0, 0, ^{a f} _j ^{−a 0}

M ] ; exit

else return end

Pro edureNJDS

This pro edure is dual to the PJDS one. If

a _f < a ₀

^{and onditions (1.25)}

and(1.26) areveried,the negativejerk degeneratesolution(1.24)isimposed

and the mainprogram isstopped.

begin if ¹ ₂ ^a

2 0 −a ² _f

j M + v 0 − v f = 0 and

1 6

a ² _f

j _M ² − ² _{3 j} ^{a 2 3 0}

M − ¹ ₂ ^a _j ^{2 0 2 a f}

M − ^{v 0} _{j M} ^{a 0} + ^{v 0} _j ^{a f}

M − s f = 0 then [t ₁ , t ₂ , ¯ t _f ] = [0, 0, ^{a 0} _j ^{−a f}

M ] ; exit

else return end

Pro edurePJS

First, all the positive real roots of quarti equation (1.16) are omputed and

stored in an array

T

^{. Then} expressions (1.14) and (1.15) are used to deter- mine a feasiblesolution. If three values of

t ₁

^,

t ₂

^{, and}

t ¯ _f

^satisfying inequalities

0 ≤ t 1 ≤ t 2 ≤ ¯t f

^{are found the} minimum-time velo ity planning solution is obtained and the mainprogram is stopped.

begin

Compute the positive real roots of

equation (1.16)

^,

T = [t _{f 1} , t _{f 2} , . . . , t _{f l} ] with (l ≤ 4) ; if T is empty then

return

for i = 1, . . . , l do t _2i =

h 3

4 j M t ² _{f i} − ¹ ₂ (3 a f −a 0 ) t f i + ¹

4 jM (a f −a 0 ) ² +v f −v 0

i j M t f i −a f +a 0 ; if 0 ≤ t 2i ≤ t f i then

t 1i = t 2i − ¹ ₂ t ² _{f i} + ¹ ₂ ^{a f} _j ^{−a 0}

M ;

if 0 ≤ t 1i ≤ t 2i then [t ₁ , t ₂ , ¯ t _f ] = [t _1i , t _2i , t _3i ] ; exit

else

continue else

continue return

end

Pro edureNJS

This pro edure is dual to the PJS one.The quarti equation to start with is

themodied(1.16)where

j M

^issubstitutedby

−j ^M

^{.Thenallthepositivereal}

solutions of thisequation are omputed and afeasible solutionis sought.

begin

In equation (1.16) do the substitution j _M ← −j M

and compute the positive real roots

^,

T = [t _{f 1} , t _{f 2} , . . . , t _{f l} ] with (l ≤ 4) ;

if T is empty then return

for i = 1, . . . , l do t _2i =

h 3

4 j M t ² _{f i} − ¹

2 (3 a f − a 0 ) t f i + ¹

4 jM (a f − a 0 ) ² +v f − v 0

i j M t f i −a f +a 0 ; if 0 ≤ t 2i ≤ t f i then

t _1i = t _2i − ¹ ₂ t ² _{f i} + ¹ ₂ ^{a f} _j ^{−a 0}

M ;

if 0 ≤ t 1i ≤ t 2i then [t 1 , t 2 , ¯ t _f ] = [t 1i , t 2i , t 3i ] ; exit

else

continue else

continue return

end

1.3.4 Simulations results

Example 1: onsider the following data:

s _f = 3, 25

^m,

j _M = 0, 5

^m/s

³

^,

v ₀ = 0

^m/s,

a ₀ = 0

^m/s

²

^,

v _f = 2, 25

^{m/s and}

a _f = 1, 5

^m/s

²

^{. Exploiting the}

MTVP algorithmdes ribedin subse tion1.3.3the following optimalsolution

isobtained:

u _M = +j _M t ₁ = 1

^s

t ₂ = 3

^s

¯ t _f = 7

^s

The jerk, a eleration, velo ity and spa e proles, for this ase, are depi ted

in gure1.3.

Example 2: let be the ase of:

s _f = 8, 42

^m,

j _M = 0, 25

^m/s

³

^,

v ₀ = 1

^m/s,

a 0 = 0, 5

^m/s

²

^,

v _f = 2, 75

^{m/s and}

a _f = 0

^m/s

²

^{. The optimal solution is the}

following:

u _M = +j _M t ₁ = 1

^s

t ₂ = ¯ t _f = 4

^s

0 1 2 3 4 5 6 7 8

−1

−0.5 0 0.5 1 1.5 2

Time[s]

PSfragrepla ements

¯ u(t)

¯ a(t)

0 1 2 3 4 5 6 7 8

−1

−0.5 0 0.5 1 1.5 2 2.5 3 3.5

Time[s]

PSfrag repla ements

¯ v(t)

¯ s(t)

Figure 1.3: The optimal proles of jerk

u(t) ¯

^, a eleration

¯ a(t)

^{, velo ity}

¯ v(t)

^{, and}

spa e

s(t) ¯

^forexample1.

See gure1.4for the optimal

u(t) ¯

^,

¯ a(t)

^,

v(t) ¯

^and

s(t) ¯

^proles.

0 0.5 1 1.5 2 2.5 3 3.5 4

−0.4

−0.2 0 0.2 0.4 0.6 0.8 1

Time[s]

PSfragrepla ements

¯ u(t)

¯ a(t)

0 0.5 1 1.5 2 2.5 3 3.5 4

0 1 2 3 4 5 6 7 8 9

Time[s]

PSfragrepla ements

¯ v(t)

¯ s(t)

Figure 1.4: The optimal proles of jerk

u(t) ¯

^, a eleration

¯ a(t)

^{, velo ity}

¯ v(t)

^{, and}

spa e

s(t) ¯

^forexample2.

1.4 Minimum-time onstrained velo ity planning

Thisse tionexplains apro edurewhi hhasappearedforthersttime in[17℄.

Theproposedmethodsolvesagain the minimum-time velo ityplanning prob-