Nonholonomic path following with fastly oscillating controls

Bronisław Jakubczyk

Institute of Mathematics, Polish Academy of Sciences00-956 Warsaw, Sniadeckich 8, Poland

Abstract— We outline a method of finding sinusoidal controlswhich assure a semi-exact path following. The method isdesigned primarily for non-admissible paths and controllablenonholonomic systems. The fastly oscillating sinusoidal controlsgenerate (in approximate sense) new control variables whichenable to control new directions. In this way the systembecomes, approximately, fully controllable and any path canbe followed with arbitrary high precision.

I. INTRODUCTION

Consider a control system

Σ : x = X0(x) +∑

i=1,...,m

uiXi(x)

with state x(t) ∈ M and control u = (u1, . . . , um) ∈ Rm.The state space M is an open, connected subset of Rn or adifferentiable manifold of dimension n.

Suppose that M = Rn and consider a continuous path

γ : [0, 1]→M, τ 7→ γ(τ).

Let ε > 0 be fixed and let x(0) = x0 be an initial state suchthat |γ(0) − x0| < ε (here |x| denotes the Euclidean normin Rn). We define the semi-exact path following property ofΣ as the existence, for any such path γ and any ε > 0, ofa control u : [0, T ] → Rm and an increasing C1-functionτ(t) (time rescaling) such that τ(0) = 0, τ(T ) = 1 and theresulting trajectory of Σ has the property

|x(t)− γ(τ(t))| < ε, t ∈ [0, T ].

Here T depends of the path and on ε.For simplicity of exposition we will consider driftless

systems, i.e. we take X0 = 0. For such systems it is wellknown that if the vector fields X1, . . . , Xm are smooth andthey generate the Lie algebra of full rank at any point in M ,then the system is completely controllable and, moreover,it has the semi-exact path following property. However,the design of appropriate controls for given a path γ is achallenge, in general. What is known is that for systemswhich have a special structure like chained form or theLie algebra generated by X1, . . . , Xm is nilpotent, relativelyefficient methods can be designed using piecewise constantcontrols (see e.g. [3]) or sinusoidal controls (cf. [4]). Thesemethods are not simple in any way, unless the system is ofsmall dimension and one needs to use Lie brackets of ordernot higher then two.

We propose a method which uses harmonic controls andthe Volterra expansion of the system output, with respect

to controls. This expansion uses iterated integrals of controlvariables (representing the ”increments” of the ”independentvariable” u(·)), while the iterated derivatives of the outputfunction along the control vector fields Xi define coefficientsof the expansion (representing the system properties). In con-trast to piecewise constant controls, where one needs to applythe Baker-Campbell-Hausdorff formula and its generaliza-tions, here the iterated integrals are easily computable. Anal-ogously, computing the coefficients requires only (iterative)differentiating and no integrating of differential equations isneeded in the whole procedure.

In principle, the method guarantees arbitrary high preci-sion. However, there is a pay for this. If we fix a bound onthe values of the control, u(t) ≤ Umax, and we want to havehigh precision, the time T needed for following the path γis much higher then the nominal time 1 used for the pathγ. Alternatively, if we want to have T small we should uselarge Umax. This phenomenon depends very much of howbig is the order of Lie brackets of Xi which we have to usein order to span all tangent directions in the state space.

The idea of the procedure is as follows. Given a path γ, wemay choose points x0, . . . , xN on the path, with xi = γ(τi)and

τ0 = 0 < τ1 < τ2 < · · · < τN = 1

so that |γ(τi) − γ(τi−1)| < ε′ := ε/2. The i-th step of theprocedure consists of choosing a control which will steerthe system from a point in a ε′-neighbourhood of xi−1to ε′-neighbourhood of xi. To this aim we should find asinusoidal control which creates the ”average” velocity ofx(t) approximately parallel to the vector vi = xi−xi−1 andmove with this velocity for sufficiently long time. A priori,this seems impossible if the vector vi is not admissible forΣ, that is vi 6∈ D(xi), where

D(x) = span{X1(x), . . . , Xm(x)}.

However, as it can be shown, fast oscillating harmoniccontrols may produce an average velocity which is closeto [Xi, Xj ](x) or even to higher order Lie brackets of thevector fields X1, . . . , Xm. Similarly, linear combinations ofsuch Lie brackets can be approximately attained as ”averagevelocity” of Σ, with appropriate fastly oscillating sinusoidalinputs.

A more general version of the path following problem ispath following in the output space. Then the system equations

Proceedings of the 9th International Workshopon Robot Motion and Control, Wasowo Palace,

Wasowo, Poland, July 3-5, 2013

978-1-4673-5511-7/13/$31.00 ©2013 IEEE 99

are supplemented with an output equation

y = h(x),

where y ∈ Y = Rp is the output (observation) variable andh : M → Rp is a smooth map. The desired path is thengiven in the output space,

γ : [0, 1]→ Rp,

and ε-path following means the fulfillment of the inequality|y(t) − γ(τ(t))| < ε. The method outlined in the presentpaper works in this case, too.

II. MOTIVATING EXAMPLE

We outline the method on the example of kinematic wheel(also known as unicycle or planar rigid body). We considerthe kinematic path following problem. The kinematic equa-tions of the unicycle (kinematic wheel) are

x = u1 cos θ,

y = u1 sin θ,

θ = u2,

where (x, y) are planar coordinates of the geometric centerof the unicycle and θ is the angle of its axis with respectto the x-axis of the coordinate system. The state space isM = R2 × S1, where S1 is the unit circle representingthe space of the values of θ. The ”controls” are the linearvelocity u1 and the angular velocity u2 of the unicycle. Anon-admissible path for such a system is γ : [0, 1] → M ,γ(τ) = (x(τ), y(τ), θ(τ)) such that (x(τ), y(τ)) is notparallel to the vector (cos θ(τ), sin θ(τ)). This is simply thepath which does not respect the ”moving without slipping”condition. The problem is how this path can be followed,with arbitrary precision, using a true motion without slipping.

In order to solve the problem we first note that one canadd an artificial control u3 and consider the extended controlsystem

x = u1 cos θ + u3 sin θ,

y = u1 sin θ − u3 cos θ,

θ = u2.

Clearly, this system is fully controllable which means that thespace of available velocities (x, y, θ) is the whole space, withthe non-slipping condition removed. Notice that the controlvector fields for the original and the extended system areX1, X2 and X1, X2, X3, respectively, where

X1 = cos θ∂

∂x+ sin θ

∂

∂y, X2 =

∂

∂θ,

X3 = sin θ∂

∂x− cos θ

∂

∂y.

Here we write the vector fields Xi as differential operators.In coordinate form we have

X1 =

cos θsin θ

0

, X2 =

001

, X3 =

sin θ− cos θ

0

.

The third vector field was not chosen at random, it is theLie bracket of the first two:

X3 = [X1, X2].

It produces a velocity perpendicular to the linear velocity ofthe original system. We claim that a movement of the systemalong the trajectory X3 is possible, approximately, and it isproduced by the control

u(t) = (sinωt, cosωt).

Claim If the control is as above and the frequency ω islarge enough then the system moves, approximately, in thedirection X3 = [X1, X2] at the speed 1

2ω per unit of time.

From the claim we deduce that the additional control u3is achievable, with the fastly oscillating control as above.The higher is the frequency ω, the higher is the precision offollowing a trajectory of X3 = [X1, X2]. Thus, any trajectoryof X3 can be approximately followed at the cost of havingto use sufficiently long time (or sufficiently high amplitudeof the oscillating control).

Another example is a rolling disc, where in addition to thecoordinates (x, y, θ) of the position of the geometric centerand its angle with respect to the x-axis we add the ”rollingangle” φ which is the angle of a fixed ray in the disc withrespect to the vertical ray. The kinematic equations are

x = u1 cos θ,

y = u1 sin θ,

θ = u2,

φ = u1.

This system is also a particular case of Σ, with X0 = 0 and

X1 = cos θ∂

∂x+ sin θ

∂

∂y+

∂

∂φ, X2 =

∂

∂θ.

X3 = sin θ∂

∂x− cos θ

∂

∂y.

We can extend it to a fully controllable system by addingtwo controls u3, u4 and two corresponding vector fields

X3 = [X1, X2] = sin θ∂

∂x− cos θ

∂

∂y,

X4 = [X2, [X1, X2]] = cos θ∂

∂x+ sin θ

∂

∂y.

In coordinate form we have

X1 =

cos θsin θ

01

, X2 =

0010

,

X3 =

sin θ− cos θ

00

, X4 =

cos θsin θ

00

.

We claim that X3 and X4 can be taken as artificial controlvector fields which can be approximately realized with fastlyoscillating sinusoidal controls.

100

III. VOLTERRA EXPANSION

In order to outline our idea in the general context weconsider a system

Σ : x =∑

i=1,...,m

uiXi(x), y = h(x), x(0) = x0,

on a differential manifold M , where x(t) ∈ M is its state,u(t) ∈ Rm is control, Xi are vector fields on M andh : M → Rp is an output map. All data: the manifoldM , the vector fields Xi on M and the function h areassumed to be of class C∞. The variable y can be calledobservation and h an observable. The controls (inputs) ui areassumed piecewise continuous functions of time, defined onan interval [0, T ]. Later we will mainly use periodic harmonicfunctions as controls.

Our general aim is to analyze the response of the systemto the input function u = (u1, . . . , um), given on [0, T ]. Theresponse will be measured, in general, as the value y(T )of the output function at the final time of action of theinput. We tacitly assume that the trajectory x and the outputare well defined on the whole interval [0, T ]. The systemis represented by the vector fields X1, . . . , Xm, the outputfunction h, and the initial point x0, all assumed to be fixed.

The starting point to our analysis is the Volterra typeexpansion of the output. Denote by ‖u‖∞ = supt∈[0,T ] |u(t)|the supremum norm of u.

Theorem III.1 Let y(t) = h(x(t)). If M , Xi, and h areanalytic and the product T ‖u‖∞ is sufficiently small, wehave a converging expansion

y(T ) = V0 + V1 + V2 + · · · , (V 1)

withVn =

∑|I|=n

SI II(u), (V 2)

where V0 ∈ Rp and, for n ≥ 1, the sum defining Vn is takenover all multi-indices I = (i1, . . . , in) with 1 ≤ i1, . . . , in ≤m. The vectors SI ∈ Rp are the iterated derivatives of thecomponents of the vector valued function h along the vectorfields Xi1 , . . . , Xin , evaluated at x0,

SI = Si1...in = (Xi1 · · ·Xin(h))(x0). (S)

The numbers Ii1...in(u) are iterated integrals of the compo-nents (u1, . . . , um) of the control u : [0, T ]→ Rm,

II = Ii1...in(u) =

=

∫ T

0

∫ s1

0

· · ·∫ sn−1

0

ui1(s1) · · ·uin(sn)dsn · · · ds1.

The iterated integrals can be equivalently written as multi-integrals over simplices,

Ii1...in(u) =

∫T>s1>···>sn>0

ui1(s1) · · ·uin(sn)dsn · · · ds1.

(I)Note that the vectors SI depend on the system data and

the initial point (they do not depend on the control). On

the contrary, the numbers Ii1...in(u) depend on the controlu : [0, T ]→ Rm, only. Thus we have

Separation principle Fix the control interval [0, T ] andconsider the effect of the control on a scalar output y = h(x)at the final time. The data needed for computing y(T ) as aresponse to a given input u : [0, T ] → Rm are two infinitefamilies of numbers

S = {Si1...in}, I = {Ii1...in}, (SI)

where n ≥ 0, i1, . . . , in ∈ {1, . . . ,m}, and Ii1...in =Ii1...in(u). The output is given by the bilinear expression

y(T ) =∑n≥0

∑1≤i1,...,in≤m

Si1...in Ii1...in (V )

which can be interpreted as a scalar product of S and I.Here, for n = 0, we take S∅ = h(x0) and I∅ = 1. The sameis true for vector output, with Si1...in being vectors.

Remark If we restrict the control to a subinterval [0, t] ⊂[0, T ] and change the iterated integrals into

Iti1...in(u) =

∫t>s1>···>sn>0

ui1(s1) · · ·uin(sn)dsn · · · ds1,

we get an analogous formula for the response at t,

y(t) = V t0 + V t

1 + V t2 + · · · ,

withV tn =

∑|I|=n

SI ItI(u).

Remark The system coefficients of the Volterra expansion(V1), (V2) satisfy the following bounds

|Si1···in | ≤ CRnn!,

where C and R are positive constants depending on theanalytic vector fields X1, . . . , Xm, the analytic function hand the point x0. The control coefficients satisfy

|Ii1···in(u)| ≤ DTn‖u‖n(n!)−1,

where D is a positive constant depending only on m and‖u‖ is the supremum norm

‖u‖ = ‖u‖∞ = supt∈[0,T ]

maxi=1,...,m

|ui(t)|.

It is clear that these two bounds imply that the series (V) isconvergent and satisfies the estimate

|Vn| ≤ Aρn(T‖u‖)n, (E)

where A = CD > 0 and ρ = mR depend only on thesystem data and x0. Thus, the initial part of Volterra series(V1), (V2) taken up to order n approximates y(T ) (and alsothe values y(t) with t ∈ [0, T ]) with precision of order εn+1,where ε = ρT‖u‖ should be taken small. This constant canbe manipulated by choosing appropriate amplitude ‖u‖ ofthe control and its time of action T .

We will see later that for sinusoidal control its frequencyω also appears in the precision constant ε and the largerfrequency gives smaller ε.

101

IV. SECOND ORDER TERMS

The Volterra expansion is not perfectly suitable for ourpurpose and it can be transformed into another, more suitableexpansion. In this paper it will be enough to use the first non-trivial term in this new expansion. To see how to transformthe first nonvanishing term we begin with considering thefirst three terms

V0 = h(x0), V1 =∑

i=1,...,m

(Xih)(x0)

∫ T

0

ui(s)ds,

V2 =∑

i,j=1,...,m

(XiXjh)(x0)

∫ T

0

∫ s1

0

ui(s1)uj(s2)ds2ds1,

where Xih is the derivative of h along the vector field Xi

and XiXjh = Xi(Xjh) is the iterated derivative of h alongXj and then along Xi.

Proposition IV.1 If the first order integrals∫ T

0ui(s)ds van-

ish thenV2 =

1

2

∑1≤i<j≤m

S[i,j]I[i,j], (V 2)

where S[i,j] is the derivative at x0 of h along the Lie bracket[Xi, Xj ],

S[i,j] = ([Xi, Xj ]h)(x0),

and

I[i,j] =

∫ T

0

∫ s1

0

(ui(s1)uj(s2)− uj(s1)ui(s2))ds2ds1.

Proof: The proof is instructive and so we provide thedetails. We can write V2 as two sums:

V2 =1

2

∑i,j=1,...,m

(XiXjh)(x0)

∫ T

0

ui(s1)ds1

∫ T

0

uj(s2)ds2

+1

2

∑i,j=1,...,m

(XiXjh)(x0) ·

·∫ T

0

∫ s1

0

(ui(s1)uj(s2)− uj(s1)ui(s2))ds2ds1.

Let∫ T

0ui(s)ds = 0. Then the first sum vanishes and V2

can be written as

V2 =1

4

∑i,j

((XiXjh)(x0) + (XjXih)(x0)) I[i,j] +

+1

4

∑i,j

([Xi, Xj ]h)(x0) I[i,j],

where [Xi, Xj ]h = (XiXj −XjXi)h is the derivative of halong the Lie bracket [Xi, Xj ]. The first sum in the aboveexpression vanishes, since the two terms with fixed i and jcancel each other. Thus V2 takes the simpler form

V2 =1

2

∑1≤i<j≤m

([Xi, Xj ]h)(x0) I[i,j] =1

2

∑i<j

S[i,j]I[i,j]

(note that the terms with j > i are equal to the ones withi < j).

Remark Note that our notation has a deeper meaning.Namely, the expression in Proposition IV.1 can be written as

V2 =1

2

∑i<j

(Sij − Sji)(Iij − Iji).

Instead of repeating the symbols S and I in each expressionwe can use a shorter notation

S[i,j] = Sij−ji := Sij − Sji, I[i,j] = Iij−ji := Iij − Iji

and then V2 = 12

∑i<j S[i,j]I[i,j], where [i, j] = ij − ji

should be treated as a commutator (formal Lie bracket) offormal, noncommuting letters (variables) i and j.

The control coefficients can easily be computed for sinu-soidal controls. An easy calculation gives

Proposition IV.2 (a) If the components ui and uj are sinu-soidal on [0, T ],

ui(t) = sin(kωt), uj(t) = cos(lωt),

with nonzero integers k, l, and T being a multiple of theperiod 2π/ω, then

I[i,j] =

{0, if l 6= kTkω , if l = k.

If T is not a multiple of the period and the phases arearbitrary, then

Proposition IV.3 (a) If the components ui and uj have thesame frequency, i.e.,

ui(t) = sin(kωt+ φ), uj(t) = sin(kωt+ψ), t ∈ [0, T ],

then

I[i,j] =T

kωsin(ψ − φ)

(1− 1

kωTsin(kωT )

).

In particular, if kωT >> 1 then I[i,j] ' Tkω sin(ψ − φ).

(b) If the frequencies are different, i.e.,

ui(t) = sin(kωt+ φ), uj(t) = sin(lωt+ ψ), k 6= l,

then|I[i,j]| ≤

T

kω

8

mωT.

Multiplying a given control by a constant A multiplies theintegrals Ii1i2 and I[i,j] by factor A2. Thus, if

ui(t) = A sin(kωt), uj(t) = A cos(kωt)

and T is a multiple of the period 2π/ω then

I[i,j] =A2T

kω.

We now address the question if the control coefficientsI[i,j] in the formula (V2) can be chosen independently, forany i < j, by designing appropriate sinusoidal controls. It

102

is enough to use sinusoids with several different frequenciesωk = kω, where k will be different integers. This is sug-gested by Proposition IV.2 showing that the only interactingfrequencies are of the form (kω,−kω).

Denote by e1, . . . , em the unit versors in the control spaceRm. Let the control be of the form

u(t) =∑i<j

Uij(t), (U1)

where

Uij = Aij(sin(kijωt)ei + cos(kijωt)ej) (U2)

and all kij are integers with pairwise different absolute values|kij |. Note that each control Uij engages the components uiand uj , only. However, the same component ui appears inseveral Uij , with different frequencies kijω. From Propo-sition IV.2 it follows that, for such control and T being amultiple of 2π/ω,

I[i,j] =A2

ijT

kijω.

Given ω, T and the ”frequencies” |kij |, we see that thecoefficients I[i,j] can be chosen arbitrary, by appropriatechoice of the amplitudes Aij and the signs of kij .

The following conclusion can be drawn from the abovepropositions.

Corollary IV.4 If the control is of the form (U1), (U2),where the integers |kij | are pairwise different and T is amultiple of 2π/ω, then V1 in the Volterra expansion is zeroand second order term V2 ∈ Rp is

V2 =∑i<j

S[i,j]

A2ijT

kijω.

Choosing appropriate amplitudes Aij and signs of kij onecan obtain V2 as any vector in the subspace

span{S[i,j] : 1 ≤ i < j ≤ m}

of the output space Rp. Thus the output can be approximatelydriven in any direction in this subspace.

Suppose now that the system has the following 2-stepoutput controllability property:

span{Si(x), S[j,k](x) : 1 ≤ i, j, k ≤ m} = Rp,

for any x in M (or in an appropriate subset in M ). Let apath γ : [0, 1] → Rp be given. Then the following strategyof approximate following the path can be used. We choosea precision constant ε > 0. Let 0 = τ0 < τ1 < · · · < τN = 1be such that |γ(τi) − γ(τi−1)| < ε′ := ε/2. Denote yi =γ(τi). In the i-th step of the procedure we will choose acontrol which will move the output from a point in a ε′-neighbourhood of yi−1 to ε′-neighbourhood of yi. To thisaim we should define a sinusoidal control which createsthe ”average” velocity of y(t) approximately parallel to thevector v = vi = yi − yi−1. Let x = xi−1 be a state reachedby the system in the preceding step. From the 2-step output

controllability it follows that there are constants ai and bij ,i < j, such that

v =∑

1≤i≤m

aiSi(x) +∑

1≤i<j≤m

bijS[i,j](x).

We first choose a ”fast oscillating” control of the form (U1),(U2) so that I[i,j](u) = bij(x). Then we apply a constantcontrol u = (u1, . . . , um) such that Ii(u) = ai(x), whereIi(u) = Tui. This moves the system output in direction v,approximately.

In the next step we replace the point x = xi−1 with apoint x = xi reached by the system in the preceding stepand apply the procedure, again.

Remark The use of the second order terms is relativelywell understood for optimal control, if the vector fieldsX1, . . . , Xm and Lie brackets [Xi, Xj ] span the wholetangent space to M . Already Brockett [1] showed that, for acanonical system with dimM = m+m(m− 1)/2, optimalcontrols are sinusoidal with frequencies ω, 2ω, . . . , kω. Itwas recently proved [2] that the fastly oscillating asymptoti-cally optimal control which realizes semi-exact path follow-ing (for a non-admissible path) is sinusoidal, with harmonicoscillations in two dimensional subspaces.

In order to use higher order terms (higher order Liebrackets), say the terms of order n, one should designsinusoidal signals which annihilate all iterated integrals up toorder n-1 and give nontrivial Ii1···in(u). This can be done ina systematic manner as we will show in a future paper. Then,instead of the sum (V2) one obtains a more complicated sumwhich can be written in a compact form using so called Liepolynomials. In particular, for n=3 this sum is

V3 =1

6

∑i,j,k

S[i,[j,k]]+[[i,j],k] I[i,[j,k]]+[[i,j],k],

where i, j, k run over all numbers 1, . . . ,m and

S[i,[j,k]]+[[i,j],k] = Sijk − Sikj − Sjki + Skji

+Sijk − Sjik − Skij + Skji.

The term I[i,[j,k]]+[[i,j],k] is given by the analogous sum ofiterated integrals.

Acknowledgment. This research was partially supportedby the Polish Ministry of Research and Higher Education,grant NN201 607 540.

REFERENCES

[1] R.W. Brockett, ”Control theory and singular Riemannian geometry”.In New Directions in Applied Mathematics, pages 11-27, Springer-Verlag, New York, 1981.

[2] J.-P. Gauthier, B. Jakubczyk and V. Zakalyukin, ”Motion Planning andFastly Oscillating Controls”, SIAM J. Control Optimiz. vol. 48 No.5(2010), pp. 3433-34-48.

[3] G. Lafferriere and H.J. Sussmann, ”Motion planning for controllablesystems without drift”. In IEEE International Conference on Roboticsand Automation 1991, pages 1148-1153.

[4] R.M. Murray and S.S. Sastry, ”Nonholonomic motion planning: steer-ing using sinusoids”, IEEE Trans. on Automatic Control, vol. 38, No.3 (1993) pp. 700-716.

103