XxA, yA, kA, ˙kA
of 0.2 m/s. The red line represents the referen e traje tory γ 0 and the blue
line therobot observed position. Inthe middleofthe testthe robot hasbeen
movedwitharodtotesttherobustnessofthe ontroller,thisexplainthelarge
transient error present in the gure. In gure 4.8-b), the norm of the
(x, y)
omponent of the tra king error is showed; the spike on time
t = 40
it isdueto the test ofthe robustness of the ontroller.
Figure 4.9-a) shows another experiment where the desired traje tory is a
splinewhi h hasbeen reparameterized with onstant speed
0.15
m/s.Theas-so iated tra king error is shown in gure 4.9-b). Remark that the evaluation
of fun tions
γ i
in (4.7) require the use of a re ursive fun tion. If fun tionλ
−0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4
−0.5
−0.4
−0.3
−0.2
−0.1 0 0.1 0.2 0.3 0.4
PSfragrepla ements
a)
0 10 20 30 40 50 60
−0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4
b)
Figure 4.8: a) Referen e and a tual traje tory for a ir le b) the norm of the
(x, y)
omponent ofthe tra kingerror with respe tto time.−0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6
−0.5
−0.4
−0.3
−0.2
−0.1 0 0.1 0.2 0.3 0.4 0.5
PSfragrepla ements
a)
0 10 20 30 40 50 60
−0.005 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04
b)
Figure 4.9: a) Referen e and a tual traje tory for a omposite spline b) the
norm ofthe
(x, y)
omponent ofthe tra king errorwith respe tto time.rea hes
0
in nite timeτ
, then the maximum order of re ursion is given bythe ratio
τ
T
(re all thatT
is the replanning time). Sin e the order ofre ur-sion is deterministi , the proposed ontrol law an be implemented in a real
time ontroller. Parameter
T
mustbe arefully hosen. In fa t, on one hand,by(4.8), (4.9), redu ing
T
improvesthe tra king performan es. Onthe other hand,it in reases the ratioτ
T
, the number of re ursions neededto implementthe ontroller and the omputational eort.
4.2 Iterative output replanning for at systems
These tion onsiderstheoutputtra kingproblemfornonlinearsystemswhose
performan e output is also a at output of the system itself. A desired
out-put signalis sought onthe a tual performan e outputbyusinga feedforward
inverse input that is periodi ally updated with dis rete-time feedba k of the
sampledstateofthesystem.Theproposedmethodisbasedonaniterative
out-putreplanningthatusesthedesiredoutputtraje toryandthesampledstateto
replananoutputtraje torywhose inverseinputhelpsinredu ing thetra king
error.ThisiterativereplanningexploitstheHermiteinterpolatingpolynomials
to a hieve an overall arbitrarily smooth input and a tra king error that an
bemadearbitrarilysmall ifthe state samplingperiodissu iently small and
mildassumptionsare onsidered.Somesimulation resultsarepresentedforthe
ases ofan uni y le anda one-trailer systemae tedbyadditive noise.
4.2.1 Problem statement
Consider the nonlinear ontrolled system
˙x = f (x, u) ,
(4.33)with
x ∈ C(R, R n )
,u ∈ C(R, R m )
. System (4.33) is at if there exists anoutput fun tion
y
su h that the system statex(t)
and the inputu(t)
an bewritten asafun tion of
y
and itsderivativesupto anite order, evaluatedat timet
. Morepre isely the following denition anbe given (see [52℄).Denition 12 System (4.33) isat ifthere exist aat output
y
of dimensionm
, two integersr
ands
andmappingsψ
fromR n × R m(s+1)
toR m
,of rankm
in a suitably hosen outputsubset, and
(φ 0 , φ 1 )
fromR m(r+2)
toR n × R m
, ofrank
m + n
in a suitable open subset, su h thaty = (y 1 , . . . , y m ) = ψ(x, u, ˙u, . . . , u (s) ) ,
(4.34)implies that
x = φ 0 (y, ˙y, . . . , y (r) ) , u = φ 1 (y, ˙y, . . . , y (r+1) ) ,
(4.35)
the dierential equation
dφ 0
dt = f (φ 0 , φ 1 )
being identi ally satised.In this way, fun tion
φ 0
represents the statex
with the outputy
and itsderivatives up to the order
r
. Fun tionφ 1
represents the inputu
with theoutputand its derivatives upto the order
r + 1
.Forsimpli ity,fora
C n
fun tionf
weusethenotationf ¯ n = (f, f (1) , . . . , f (n) )
,to denotethe ordered set ontaining fun tion
f
and its time derivativesupto the ordern
.If
φ 1
issu iently regular, dierentiating (4.35),one obtainsfun tionsφ i
,su h that, forany
i ≥ 1
u (i−1) = φ i (¯ y r+i ) ,
(4.36)i.e., the input derivatives an be expressed as a fun tion of the output and
its derivatives. Similarly, if
ψ
is su iently regular, dierentiating (4.34),one obtains fun tionsψ i
, su h that,for anyi ≥ 0
y (i) = ψ i (x, ¯ u s+i ) ,
(4.37)with
ψ 0 = ψ
, whereψ
is given in (4.34). Combining (4.36) and (4.37), thefollowing identityholds
∀i ≥ 1
u (i−1) = φ i (ψ 0 (x, ¯ u s ), . . . , ψ r+i (x, ¯ u (s+r+i) )) .
(4.38)It is well known that tra king and motion planning problems an be easily
solvedforatsystems,seeforinstan e hapter7of[52℄.Inthisse tionwestudy
the tra king problemfor system (4.33),in presen eof a bounded disturban e
added to the nominalvelo ityof the state:
˙x(t) = f (x(t), u(t)) + η(t) ,
(4.39)where
η
isa disturban e signalsu h thatkη(t)k ≤ N, ∀t ∈ R .
(4.40)The performan e outputofsystem(4.39) is givenby
y = ψ(x, u) .
(4.41)We assumethat
y
is a at output for system (4.39) when no noise is present(i.e.,
η = 0
). In this ase, from (4.34) and (4.35), it follows that the outputsignal
y
satisesy = ψ(φ 0 (¯ y r ), φ 1 (¯ y r+1 )) .
(4.42)Notethat the form(4.39) mayberestri tive sin e the disturban e
η
enters asa pure additive term. Thisformdoesnot in lude, for instan e, ases in whi h
a disturban emultiplies the state
x
or the inputu
.We assumethatthe full systemstateis a quiredperiodi ally, with a
sam-pling period equal to
T > 0
. In this way, the feedba k ontrol relies on thedis rete-time observed sequen e
x(kT )
,k ∈ N
. For instan e, this assumptionisreasonablewhen the systemstate isobtainedthrough a amera,using
om-putervisionte hniques.Inthis ase,asamplingtimeof
T = 0.1
se ondswouldbea typi al situation.
We study an iterative output replanning te hnique for ontrolling
sys-tem (4.39), based on Hermite interpolating polynomials, similar in spirit to
the iterative state steering method presented in [4℄. Roughly speaking, the
method is the following. A su iently regular referen e output traje tory
y d
is assigned in advan e. During ea h time interval
[kT, (k + 1)T [
, a replannedoutput
y p
is omputed su h that1.
y p
orresponds through (4.35) to an initial state whi h is the same asx(kT )
, i.e.x(kT ) = φ 0 (y p (kT ), ˙y p (kT ), . . . , y p (r) (kT )) .
2. thereplannedoutput
y p
onvergestothedesiredoney d
attime(k + 1)T
,i.e.,
y p ((k + 1)T ) = y d ((k + 1)T )
.The ontrol isgiven a ording to (4.35),
∀t ∈ [kT, (k + 1)T [
, byu(t) = φ 1 (y p (t), ˙y p (t), . . . , y (r+1) p (t)) .
Sin e the system is ae ted by additive noise and in interval
[kT, (k + 1)T [
open loop ontrol is used, at time
(k + 1)T
the system outputy((k + 1)T )
isdierent from
y d ((k + 1)T )
. Hen e, the above step is repeated, nding a newreplanned traje tory
y p
, thatwoulddrive theoutputofthe nominalsystemtoy d
attime(k +2)T
.Again,forthepresen eofnoise,attime(k +2)T
thea tualsystemoutput is dierent from the referen e traje tory and a new traje tory
isreplanned.Sin ethe replannedtraje tories onverge tothereferen e
y d
,thesystem output is driven towards the desired output and the tra king error is
kept limited despite the presen eof a disturban e. Thismethod isillustrated
in gure4.10, while gure4.11shows the orresponding ontrol s heme.
Weprovethatthetra kingerror anbemadearbitrarilysmallifthe
replan-ningtime
T
is hosensu iently small.Moreover,we showthatthereplannedoutput
y p
anbe hoseninsu hawaytohaveanarbitrarydegreeof ontinuityon the resulting inputfun tion.
4.2.2 An Hermite interpolation problem
Consider the following problem.
Problem 7 (Replanning problem) Givenatsystem (4.33),anoutput
ref-eren e traje tory,
y d ∈ C r+l (R, R m )
, aninitialstatex 0 ∈ R n
andinitialvaluesfor the input and its derivatives
u 0 , u (1) 0 , . . . , u (l−1) 0
, nd an output referen etraje tory
y p ∈ C r+l (R, R m )
su h that the followingproperties holdy d (0)
y d (T )
y d (2T ) y
y p y(T )
y(2T ) y(3T ) y d
Figure 4.10: The iterative replanning method. The gureshows the referen e
outputtraje tory
y d
,the a tualsystemoutputy
andthe replannedtraje toryy p
.Iterative output replanning
Flatness -based inversion
PSfragrepla ements
y d (t) y p (t) ˙x = f (x, u) + η
y = ψ(x, u)
u(t) y(t)
T
x(t) x(kT )
Figure 4.11: The iterative ontrol s heme for the traje tory tra king of a at
system.
a)
φ 0 (¯ y r p (0)) = x 0
,i.e.,x 0
is the initialstate of the system traje tory that hasy p
as output,b)
u (i) 0 = φ i+1 (¯ y r+i+1 p (0))
,i = 0, . . . , l − 1
i.e.,u (i) 0
is the initial value of thei
-th derivative of the ontrol for the system traje tory whi h hasy p
asoutput,
)
y p (t) = y d (t), ∀t ≥ T
,whereT
isagiven positive onstant,i.e. fun tiony p
onverges to
y d
attimeT
.For any
l ∈ N
, letΨ 0 , Ψ 1 , . . . , Ψ r+l
, be ve tors inR m
and set matrixΨ = (Ψ 0 , Ψ 1 , . . . , Ψ r+l )
. Consider the interpolation problemofdetermining a fun -tionπ Ψ,T ∈ C r+l ([0, T ], R m )
that satisesthe two onditionsd i
dt i π Ψ,T (0) = Ψ i , i = 0, . . . , r + l ,
(4.43)d i
dt i π Ψ,T (T ) = 0, i = 0, . . . , r + l .
(4.44)Condition (4.43) requires that fun tion
π Ψ,T
have the rstr + l
derivatives equal to the olumns ofΨ
at timet = 0
, while ondition (4.44) requires thatall derivatives upto the
(r + l)
-thbeequal to0
attimet = T
.Thisproblembelongstothe lassofHermiteinterpolationproblems,whi h
havebeenwidelystudiedininterpolationliterature.Itssolution anbewritten
in the form
(π Ψ,T ) i (t) =
r+l
X
k=0
A T,k (t) (Ψ k ) i ,
(4.45)where theHermite interpolationfun tion
A T,k
isthe minimum degreepolyno-mialthat satises onditions
d i
dt i A T,k (0) = δ i−k , d i
dt i A T,k (T ) = 0 ,
where
δ i =
( 1
ifi = 0 ,
0
otherwise.Thesepolynomialshavedegree
2(r + l + 1)
and anbe omputedin losedformusing aresultpresented in [69℄:
A T,k (t) = (t − T ) r+l+1 t k k!
r−k+l
X
i=0
(−t) i i!
(r + i + l)!
(−T ) r+l+i+1 (r + l)! .
(4.46)Thesepolynomials satisfythefollowing inequality,
∀t ∈ [0, T ]
|A T,k (t)| ≤ T k k!
r−k+l
X
i=0
(r + i + l)!
i!(r + l)! .
Expression(4.45) impliesthat, for any
T > 0 ˜
, thereexistsa onstantC
, su hthat,
∀T ∈ [0, ˜ T ]
,∀t ∈ [0, T ]
|π Ψ,T (t)| ≤ CkΨk .
(4.47)Figure4.12showssomeoftheHermitepolynomials
A T,k
.WeuseHermitepoly-0 0.2 0.4 0.6 0.8 1
0 0.2 0.4 0.6 0.8 1
PSfragrepla ements
A 1,0
A 1,1
A 1,2
Figure4.12: Therst three Hermitepolynomials for
r + l = 4
andT = 1
.nomials for deningthe replanned traje tory
y p
. To this end,dene fun tionψ : R ¯ n × R m(r+l+1) → R m×(r+l+1)
(z, v, v 1 , . . . , v r+l ) ; (Φ 0 , Φ 1 , . . . , Φ r+l ) ,
(4.48)
su h that
Φ k = ψ k (z, v 0 , v 1 , . . . , v k ) , k = 0, . . . , r + l ,
(4.49)where
ψ k
is dened in (4.37).In this wayΦ k
representsthek
-thderivative oftheoutputobtainedwhenthesystemstateis
z
andtheinputanditsderivatives aregivenbyv i
,i = 0, . . . , k
.Finallydenethereferen etraje toryy p
asfollowsy p (t) =
( y d (t) + π Ψ,T (t)
if0 ≤ t < T y d (t)
ift ≥ T ,
where
Ψ = ¯ ψ(x 0 , u (0) 0 , . . . , u (r+l) 0 ) − ¯y d r+l (0) .
(4.50)Fun tion
y p
solvesthe replanning problem, sin e1. itbelongs to
C r+l
,2. itsatisesproperties a) andb) be ause of onditions (4.43) and(4.50),
3. itsatisesproperty ) be ause of ondition (4.44).
Remark In this way,
x 0
represents the initial state orresponding to output fun tiony p
andu 0 , u (1) 0 , . . . , u (r+l) 0
represent the initial input and the initialinputderivativesup to the degree
r + l
.The use of Hermite interpolation allows to dene replanned traje tories that
orresponds to arbitrary onditions on the initial state, the initial input and
its derivatives.
4.2.3 Iterative ontrol law
Using the replanning method des ribed in the previous se tions, the ontrol
lawfor system(4.39) is dened asfollows
u(t) = φ 1 (¯ y p r+1 (t)) ,
(4.51)where
φ 1
is given in (4.35) and,fort ∈]kT, (k + 1)T ]
y p (t) = y d (t) + π Ψ(k),T (t − kT ) ,
(4.52)with
Ψ(0) = ¯ ψ(x 0 , u 0 , u (1) 0 , . . . , u (r+l) 0 ) − ¯y r+l d (0) ,
(4.53)and,for
k > 0
,Ψ(k) = ¯ ψ(x(kT − ), ¯ u r+l (kT − )) − ¯ y d r+l (kT ) .
(4.54)In (4.53),
x 0
represents the initial statex(0)
and the assigned onstantsu 0
,u (1) 0 , . . . , u (r+l) 0
arethe initial ontrolinputwithitsderivatives.Inotherwords, intimeinterval]kT, (k + 1)T ]
itisusedthe ontrolfun tionu
thatwoulddrivethe nominal system (4.33) along the referen e traje tory
y p (t)
. Thistraje -tory is omputed by adding the polynomial fun tion
π Ψ(k),T
to the referen etraje tory
y d
. In thiswaythe replanned traje toryy p
satisesthe propertiesa)
φ 0 (¯ y r p (kT )) = x(kT )
, i.e.x(kT )
isthe valueattimekT
of the statetraje -tory that orrespondsto
y p
,b)
y ¯ r+l p ((k + 1)T − ) = ¯ y d r+l ((k + 1)T )
, i.e.thereplanned traje toryisthesameasthe desiredtraje toryat time
(k + 1)T
.4.2.4 Main results
Arelevantpropertyisthatthe resulting ontrol fun tion
u
isC l−1
ontinuousasshownin the following proposition.
Proposition 9 The ontrol fun tion
u
denedin (4.51)belongsto lassC l−1
.Proof.Sin e
y p
isof lassC r+l
intheopensets]kT, (k+1)T [
a ordingto(4.36),the ontrolfun tionbelongsto
C l−1
inthetheunionofintervals]kT, (k +1)T [
,k ∈ N
.ItremainstoproveC l−1
ontinuityonkT
,k ∈ N
.Sin esystem(4.33)isat,bydenition(4.51) andtakingintoa ount (4.36)itfollowsthat,
∀k ∈ N
,u (i) (kT + ) = φ i+1 ( ¯ y p r+i+1 (kT + )), i = 0, . . . , l − 1 ,
moreover, by onditions (4.37), (4.54)
y (i) p (kT + ) = ψ i (x(kT − ), ¯ u s+i (kT − )), i = 0, . . . , l ,
therefore by (4.38),
u (i) (kT + ) = u (i) (kT − )
,∀i = 0, . . . , l − 1
,∀k ∈ N
, whi hproves
C l−1
ontinuity.Remark Withregardstoproposition9,itisworthnotingthatinteger
l
is,inpra ti e,afreeparameterprovidedthatasu ientlysmoothdesiredtraje tory
y d ∈ C r+l
is designed.Consequently, this impliesthatthe ontrol input ofthe proposedmethod an be hosen assmooth asne essaryor desired.The main result of this paper requires the following Lips hitz assumption
on fun tion(4.33).
Assumption 3 Givenatsystem (4.33),thereexist onstants
0 < L f , L ψ ∈ R
for whi h
∀x 1 , x 2 ∈ R n , u ∈ R m
kf (x 1 , u) − f (x 2 , u)k ≤ L f kx 1 − x 2 k ,
and the asso iated fun tion
ψ ¯
(see (4.48)) satises the following ondition,∀x 1 , x 2 ∈ R n
andu 0 , . . . , u r+l ∈ R m
k ¯ ψ(x 1 , u 0 , . . . , u r+l ) − ¯ ψ(x 2 , u 0 , . . . , u r+l )k ≤ L ψ kx 1 − x 2 k .
The following theorem statesthatitis always possible to hoosea replanning
time
T
, su iently small, su h that the output tra king error is lower thananygivenpositive onstant