Near-Identity Diffeomorphisms and Exponential ∈-Tracking and ...

Near-Identity Diffeomorphisms and Exponential

ε-Tracking and ε-Stabilization of First-Order

Nonholonomic SE(2) Vehicles

Reza Olfati-SaberControl and Dynamical Systems

Division of Engineering and Applied Science, 107-81California Institute of Technology

Pasadena, CA [email protected]

ACC02-IEEE1594

Abstract

In this paper, we address ε-tracking and ε-stabilizationfor a class of SE(2) autonomous vehicles with first-order nonholonomic constrains. We introduce a classof transformations called near-identity diffeomorphismsthat allow dynamic partial feedback linearization of thetranslational dynamics of this planar vehicle. This al-lows us to achieve global exponential ε-stabilization andε-tracking (in position) for the aforementioned classof planar vehicles using a coordinate-independent dy-namic state feedback. This feedback law is only discon-tinuous w.r.t. the augmented state. We apply our re-sults to ε-stabilization/tracking of a nonholonomic mo-bile robot.

Keywords: nonholonomic systems, nonlinear con-trol, autonomous vehicles, ε-stabilization, ε-tracking,mobile robots, dynamic partial feedback linearization,dynamic state feedback

1 Introduction

Control of autonomous vehicles is currently an impor-tant field of research. Many vehicles of interest includ-ing mobile robots [1, 2, 3], surface vessels [4], VTOLand CTOL aircraft [5, 6], hovercraft, CalTech ductedfan [7, 8], helicopters, aircraft, and underwater vehiclesare systems moving in R2 or R3. The configurationspace of these systems (in the simplest form) is SE(2)or SE(3), respectively (where SE(n) denotes the Spe-cial Euclidean group of rigid motions in Rn). All thesevehicles are control systems with first-order or second-order nonholonomic constraints.

The main purpose of this paper is to address stabi-lization and tracking problems for the dynamic modelof a class of SE(2) vehicles with first-order nonholo-nomic constraints. A similar treatment for trackingand stabilization of SE(2) vehicles with second-ordernonholonomic constraints is presented in [9].

Stabilization of the kinematic model of special exam-ples of SE(2) vehicles with first-order nonholonomicconstraints including a two-wheeled mobile robot and arolling disk have been addressed by several researchersin the past. These systems were used as benchmarkexamples for nonlinear control of nonholonomic sys-tems. Among all methods are motion planning usingsinusoids [10], applying time-varying change of coor-dinates to driftless nonholonomic systems [11], localexponential stabilization of homogeneous systems us-ing periodic inputs [12], ε-tracking for a nonholonomicintegrator [13], and finally the use of discontinuouschange of coordinates [1] and quasi-smooth dynamicstate feedback for stabilization of chained form systems[14]. Many of the aforementioned results are local, re-quire the use of small inputs, and suffer from undesiredsingularities and/or lack of stability in the sense of Lya-punov [10, 12, 1]. In contrast, our results are global, theproposed controllers are coordinate-independent, thestabilization/tracking is rather aggressive (i.e. expo-nential), the control design is directly performed on thedynamic model, and our approach is readily applicableto SE(2) vehicles with second-order nonholonomic con-straints [9] and SE(3) vehicles.

The key tool in our approach is a class of trans-formations called near-identity diffeomorphisms (NID)which allow dynamic partial feedback linearization ofthe translational dynamics of SE(2) vehicles. This inturn results in a dynamic state feedback that achievesglobal exponential ε-stabilization and ε-tracking in po-sition for the aforementioned class of SE(2) vehi-cles. As an example, we apply our results to ε-

stabilization/tracking of a wheeled mobile robot.

The outline of the paper is as follows. In section 2,near-identity diffeomorphisms are introduced and thenotions of ε-stabilization and ε-tracking under dynamicstate feedback are defined. In section 3, the dynam-ics of a planar vehicle with nonholonomic velocity con-straint is given. Our main results are presented in sec-tion 4. In section 5, the application of these results toε-stabilization/tracking of a mobile robot is provided.Finally, in section 6 concluding remarks are made.

2 Near-Identity Diffeomorphisms

Near-identity diffeomorphisms and their applicationswere first discussed in [15]. Here, we define whatwe mean by near-identity diffeomorphisms and providesome examples.

Definition 1. (near-identity diffeomorphism) Letψ(x, λ) : Rn × Rp → Rn be a smooth function andz = ψ(x, λ) be a global diffeomorphism in x for allλ ∈ Rp, i.e. there exists a smooth function φ(z, λ) :Rn × Rp → Rn such that

φ(ψ(x, λ), λ) = x, ψ(φ(z, λ), λ) = z

for all x, z, λ. We say z = ψ(x, λ) is a near-identitydiffeomorphism iff for all x ∈ Rn

ψ(x, λ) = x ⇐⇒ λ = 0

Remark 1. By uniform continuity of ψ(x, λ) at λ = 0w.r.t. x that belongs to a compact domain K, it followsthat

∀ε > 0,∃δ > 0 : ‖λ‖ ≤ δ =⇒ ‖ψ(x, λ)− x‖ ≤ ε

for all x ∈ K. In other words, for ε � 1, ψ(x, λ) is inan ε-neighborhood of x-thus the name near-identity.

Two simple examples of a near-identity diffeomorphismare as the following:

i) ψ(x, λ) = x+ λ~u, x, ~u ∈ Rn, λ ∈ R, ‖~u‖ = 1.

ii) ψ(x, λ) = x + Aλ, x ∈ Rn, λ ∈ Rp, p <n,A has full column rank.

According to remark 1, in case i) δ(ε) = ε and in caseii) δ(ε) = ε/σmax(A). In this paper, we use NID’s thatare similar to the one in i).

Now, consider a nonlinear control system with a(state,input) pair (x, u) ∈ Rn × Rm augmented with

another nonlinear system with a (input,state) pair(x, λ) ∈ Rn × Rp as the following

x = f(x, u)λ = g(x, λ)

(1)

and let z = ψ(x, λ) be a near-identity diffeomorphism.In new coordinates, we obtain the following (z, λ)-system

z = f(z, λ, u)λ = g(z, λ)

(2)

with obvious definitions of f , g.

Definition 2. (ε-stabilization) For a fix ε > 0, let λf =δ(ε) satisfy the property in remark 1. Let x0 be a desiredequilibrium point of x = f(x, u) with u = 0. We saythe dynamic state feedback

u = k(x, λ)λ = g(x, λ), λ(0) = λ0

(3)

achieves globally asymptotic ε-stabilization of x0 forthe x-subsystem in (1) iff for the closed-loop (z, λ)-system

z = f(z, λ, k(z, λ))λ = g(z, λ)

with k(z, λ) = k(φ(z, λ), λ), (x0, λf ) is a globallyasymptotically stable equilibrium in the sense of Lya-punov.

The notion of ε-tracking given a dynamic state feedbackis defined in a rather simpliar way (see [15, p. 212]).

Definition 3. (ε-tracking) For a fix ε > 0, let λf =δ(ε) satisfy the property in remark 1. Consider thefollowing nonlinear system with an (input,output) pair(u, y)

x = f(x, u)y = h(x) (4)

after applying a near-identity diffeomorphism z =ψ(x, λ) and augmenting the system in (4) with λ =g(x, λ), the dynamics of the augmented system in newcoordinates takes the form

z = f(z, λ, u)λ = g(z, λ)y = h(φ(z, λ))

(5)

Let u = k(z, λ, yd, yd, . . . , y(r)d ) be a control law that

achieves asymptotic tracking of a desired trajectoryyd(·) for (5) such that λ → λf as t → ∞. Then, wesay

u = k(x, λ, yd, yd, . . . , y(r)d ) := k(ψ(x, λ), λ, yd, yd, . . . , y

(r)d )

achieves asymptotic ε-tracking of the desired trajectoryyd(·) for (4).

3 Kinematic SE(2) Vehicles

A two-wheeled mobile robot depicted in Fig. 1 is an ex-ample of an SE(2) vehicle. A coordinate-independentrepresentation of the dynamics of this mobile robot isin the form

x = (Re1)v1R = Rωv = τ

(6)

where (x,R) ∈ R2 × SO(2) = SE(2), R is a rotationmatrix in R2 satisfying RTR = I2 with det(R) = 1.Also, v = (v1, ω)T and ω is a skew-symmetric matrixgiven by

ω =[

0 −ωω 0

]By a SE(2) vehicle, we mean a robot that its config-uration space is SE(2). Consider the following near-identity diffeomorphism

z = ψ(x, λ) := x+ λ(Re1) (7)

where λ ∈ R, ei is the ith standard basis in Rn, andR is the rotation matrix. The dynamics of a kinematicSE(2) vehicle in z-coordinates can be expressed as

z = Rλv + (Rλe1)λRλ = Rλω(λ, λ)v = τ

(8)

where v = (v1, ω)T and Rλ, ω(λ, λ) are given by

Rλ = [Re1|λRe2] , ω(λ, λ) =

0 −λω1λω

λ

λ

where [C1|C2| . . . |Cm] denotes an n × m matrix withcolumns C1, C2, . . . , Cm ∈ Rn. The properties of Rλ inthe following lemma is stated for the future use.

Lemma 1. Rλ satisfies the following properties:

det(Rλ) = λ, (Rλ)−1 = RTλ−1 (9)

The proof of Lemma 1 is very elementary. Accordingto Lemma 1, for λ 6= 0, Rλ is invertible and for λ 6= 1,Rλ 6∈ SO(2).

Remark 2. In coordinates, Rλ takes the following form

Rλ =[

cos(θ) −λ sin(θ)sin(θ) λ cos(θ)

]

We make use of the following Lemma later.

Lemma 2. Assume (z, λ) → 0 as t → ∞ and λ(t) 6=0,∀t. Then, ω(t) → 0 as t→∞ as well.

Proof: Based on property i) in Lemma 1 and the factthat λ(t) 6= 0 for all t, Rλ(t) is invertible for all t and

v = R−1λ (z − (Re1)λ)

By assumption, the right hand side of the last equationvanishes and thus v(t) = (v1(t), ω(t)) → 0 as t→ 0.

4 Main Results

In this section, we present our main ε-stabilization andε-tracking results for the class of kinematic SE(2) ve-hicles.

Proposition 1. Consider the kinematic SE(2) vehiclein (8) augmented with

λ = −cλ(λ− ε), λ(0) > ε > 0, cλ > 0

Then, applying the change of coordinates (q, p) = (z, z)as the following

q = x+ λ(Re1)p = Rλv + λ(Rλe1)

(10)

transforms the dynamics of the system into

q = p

p = Rλτ +Rλω(λ, λ) + λRλ(ω(λ, λ)− cλI)Rλ = Rλω(λ, λ)ω = τ2

(11)In addition, the (q, p)-subsystem in (11) is exact dy-namic feedback linearizable as

q = p, p = u

where q, p, u ∈ R2 by applying the following invertiblechange of control

τ = R−1λ u− ω(λ, λ)v − λ(ω(λ, λ)− cλI)e1

Proof: By direct calculation.

Now, we are ready to present our main result on ε-stabilization of kinematic SE(2) vehicles.

Proposition 2. (ε-stabilization) Any desired positionx0 ∈ R2 for the kinematic SE(2) vehicle in (6) can berendered globally exponentially ε-stable by applying thefollowing quasi-smooth dynamic state feedback

τ = −cpR−1λ (q − x0)− cdv − cdλe1

− ω(λ, λ)v − (ω(λ, λ)− cλI)λe1λ = −cλ(λ− ε), λ(0) > ε

(12)

where cp, cd, cλ > 0 are constants and q is defined in(10). In addition, ω(t) → 0 as t→∞.

Proof: Fix an ε > 0, clearly λ = ε is globally exponen-tially stable for the λ-subsystem and λ(t) ≥ ε > 0,∀t ≥0. Let q0 = x0. Based on proposition 1, q = u. Thus,applying the following feedback

u = −cp(q − q0)− cdp, cp, cd > 0

guarantees global exponential stability of (q, p) =(xd, 0). Substituting this feedback law in (1) gives thedynamic state feedback in the question. Since bothp = z and λ are exponentially vanishing, from Lemma2, it follows that ω(t) → 0 as t→∞.

Remark 3. Based on dynamics of the kinematic SE(2)vehicle in (6), if τ1 = v1 = 0, x remains invariant andin local coordinates θ = τ2. Therefore, the orienta-tion of the vehicle can be exponentially stabilized toany desired orientation without changing the position.Hence, it is sufficient to only stabilize the translationaldynamics of a kinematic SE(2) vehicle as in Proposi-tion 2 and then switch to a local controller that changesthe attitude of the vehicle to a desired attitude.

Exponential ε-tracking of a smooth desired output xd(·)can be obtained based on the following result.

Proposition 3. (ε-tracking) Let xd(t) : R → R2 bea C2 smooth desired trajectory. Then, the followingdynamic state feedback law achieves global exponentialε-tracking for the desired output (i.e. position) xd(·) ofthe kinematic SE(2) vehicle in (6)

τ = −cpR−1λ z − cdv − cdλe1

− ω(λ, λ)v − (ω(λ, λ)− cλI)λe1+ R−1

λ (cpxd + cdxd + xd)λ = −cλ(λ− ε)

(13)

where cp, cd, cλ > 0 are constants and q is defined in(10).

Proof: Let us consider the following partial state-output feedback law

u = −cp(q − xd)− cd(p− xd) + xd, cp, cd > 0

and define the output error as e = q − xd. Then, esatisfies the following output error dynamics

e+ cde+ cp = 0

Since cp, cd > 0, the output error e globally exponen-tially converges to zero. This means that x(t) globallyexponentially converges to a λ-neighborhood of xd(t)where λ exponentially converges to ε.

5 Example: A Mobile Robot

Consider the mobile robot depicted in Fig. 1 [1, 2].The robot has two rolling wheels that can be controlled

θ(x,y)

τ 1

x

y

τ 2

Figure 1: A mobile robot

independently using input torques. The dynamics ofthis nonholonomic mobile robot in coordinates is givenin equation (6).

Figures 2 and 3 show the path of the mobile robotstarting at position x = (2, 3)T for the initial orienta-tion angles θ = kπ/4, k = 0, . . . , 7. The trajectories forthe position and input torques (controls) are shown inFigure 4. These results demonstrate that the controlleraggressively stabilizes the origin for this nonholonomicmobile robot. Each trajectory exponentially convergesto a point within a distance ε = 0.01 from the desiredequilibrium xd = 0. This is sufficiently close to the ori-gin for all practical purposes. The values of the param-eters in all simulations for the mobile robot were cho-sen as cλ = 1, cp = 1, cd = 2, λ(0) = 0.5, ε = 0.01. Thetrace trajectories of this nonholonomic robot are shownin Fig. 5. Also, Fig. 6 demonstrates simulation resultsof the ε-tracking for a nonholonomic robot starting atposition (4, 4) with orientation angle π/2. The desiredtrajectory is an ellipse (x, y) = (3 sin t, 4 cos t). Clearly,the robot very quickly converges to an ε-neighborhoodof the desired trajectory.

6 Conclusion

In this paper, we introduced a class of diffeomorphismsreferred to as near-identity diffeomorphisms which al-low dynamic partial feedback linearization of the trans-lational dynamics of certain classes of SE(2) vehicles.This in turn led to global exponential ε-stabilizationand ε-tracking in position for these planar vehicles. Themain features of the obtained control laws are that theyare coordinate independent (i.e. require no switchingof the local charts), and are directly designed for thedynamic model of a vehicle as compared to the kine-matic model. We applied our results to tracking andstabilization of a wheeled nonholonomic mobile robot.The simulation results demonstrate that the dynamicstate feedback used for ε-stabilization and ε-trackingof this mobile robot is an aggressive control law (i.e.the solution converges exponential fast to the desired

position/trajectory).

References

[1] A. Astolfi, “Discontinuous control of nonholo-nomic systems,” Systems & Control Letters, vol. 27,pp. 37–45, 1996.

[2] A. Astolfi, “On the stabilization of the nonholo-nomic systems,” The 33rd Conf. on Decision and Con-trol, pp. 3481–3484, Orlando, FL, 1994.

[3] R. T. M’Closkey and R. M. Murray, “Exponen-tial stabilization of driftless nonlinear control systemsvia time-varying homogeneous feedback,” Proc. IEEEconf. on Decision and Control, pp. 943–948, San Anto-nio, TX, 1993.

[4] K. Y. Pettersen and O. Egeland, “Exponentialstabilization of an underactuated surface vessel,” Proc.IEEE Conf. on Deision and Control, pp. 1682–1687,1996.

[5] J. Hauser, S. Sastry, and G. Meyer, “Nonlin-ear control design for slightly non-minimum phase sys-tems,” Automatica, vol. 28, pp. 665–679, 1992.

[6] P. Martin, S. Devasia, and B. Paden, “A differentlook at output tracking: control of a VTOL aircraft,”Proceedings of 33rd IEEE Conference on Decision andControl, pp. 2376–2381, 1994.

[7] M. Milam and Murray R. M., “A testbed fornonlinear flight control techniques: the Caltech ductedfan,” Proc. of the IEEE Int. Conf. on Control Appli-cations, vol. 1, pp. 345–351, 1999.

[8] M. Milam, K. Mushambi, and Murray R. M.,“A new computational approach to real-time trajec-tory generation for constrained mechanical systems,”Proc. of the 39th IEEE Conf. on Decision and Control,vol. 1, pp. 845–551, 2000.

[9] R. Olfati-Saber, “Exponential ε-tracking and ε-stabilization of second-order nonholonomic SE(2) ve-hicles using dynamic state feedback,” Proceedings ofthe American Control Conference, Anchorage, Alaska,May 2002.

[10] R. Murray and S. Sastry, “Nonholonomic mo-tion planning:steering using sinusoids,” IEEE Trans.on Automatic Control, vol. 38, no. 5, pp. 700–716, May1993.

[11] J.-P. Pomet, “Explicit design of time-varying sta-bilizing control laws for a class of controllable systemswithout drift,” Systems & Control Letters, vol. 18, pp.147–158, 1992.

[12] R. T. M’Closkey and R. M. Murray, “Exponen-tial stabilization of driftless nonlinear control systemsusing homogeneous feedback,” IEEE Trans. on Auto-matic Control, pp. 614–628, May 1997.

[13] A. Bloch and S. Drakunov, “Stabilization andtracking in the nonholonomic integrator via slidingmodes,” Systems & Control Letters, vol. 29, pp. 91–99,1996.

[14] M.-C. Laiou and A. Astolfi, “Quasi-smooth con-trol of chained systems,” Proc. of the American ControlConference, San Diego, CA, June 1999.

[15] R. Olfati-Saber, Nonlinear Control of Un-deractuated Mechanical Systems with Application toRobotics and Aerospace Vehicles, Ph.D. thesis, Mas-sachusetts Institute of Technology, Department of Elec-trical Engineering and Computer Science, February2001, http://www.cds.caltech.edu/~olfati.

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=0

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=π/4

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=π/2

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=3 π/4

x1

x 2

Figure 2: Trajectories of the nonholonomic mobilerobot in (x1, x2)-plane for initial position x =(2, 3)T (v = 0) and orientation angles θ =0, π/4, π/2, 3π/4.

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=π

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=5π/4

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=3π/2

x1

x 2

0 0.5 1 1.5 2 2.50

0.5

1

1.5

2

2.5

3

3.5

θ0=7π/4

x1

x 2

Figure 3: Trajectories of the nonholonomic mobilerobot in (x1, x2)-plane for initial position x =(2, 3)T (v = 0) and orientation angles θ =π, 5π/4, 3π/2, 7π/4.

0 2 4 6 80

2

4

time (sec)

Pos

ition

x1

x2

0 2 4 6 8−50

0

50

time (sec)

Con

trol

τ1

τ2

Figure 4: Trajectories of the nonholonomic mobile robotin (x1, x2)-plane starting at x = (2, 3)T (v = 0)with angle θ = π/4

−6 −4 −2 0 2 4 6−5

−4

−3

−2

−1

0

1

2

3

4

5

x

y

(a)

−6 −4 −2 0 2 4 6−5

−4

−3

−2

−1

0

1

2

3

4

5

x

y

(b)

Figure 5: Trace trajectories of a two-wheel nonholonomicrobot with initial position (5, 4) and headingangles (a) θ = 0 and (b) θ = π/2.

0 5 10 15 20−5

0

5

time (sec)

xx

d

0 5 10 15 20−5

0

5

time (sec)

yy

d

(a)

0 5 10 15 20−30

−20

−10

0

10

20

30

time (sec)

Con

trol

Tor

ques

τ1

τ2

(b)

−6 −4 −2 0 2 4 6−5

0

5

x

y

(c)

Figure 6: (a),(b) Position trajectory and control for track-ing starting at the initial point (4, 4, π/2) and(c) The associated trace trajectories of the non-holonomic robot for initial condition in (a).