Local path following problem for time-varying nonlinear control affine systems 1

(1)

Local path following problem for time-varying nonlinear control affine systems ¹

Luca Consolini

²

Mario Tosques

³

Abstract

The local path following problem for time-varying nonlinear control affine systems is addressed and sufficient conditions for its solution are found by transforming it in a constrained non linear control problem on a suitable manifold. In the case of Chaplygin-type systems, a very simple conditions and a dynamic inversion generator of the control are exhibited. The developed theory has been applied to the kinematic models of planar and aerial vehicles.

Keywords. Path following, dynamic inversion, nonlinear affine systems, time-varying, constrained control problem, Chaplygin systems.

1 Introduction and problem formulation Consider the time-varying non linear affine control system









˙x = f (t, x) + g(t, x)u y = h(x)

x(0) = x0

u(0) = u0

(1)

where f : J × Ω → Rⁿ, g : J × Ω → R^n×m, h : Ω → R^p, with 1 ≤ p ≤ n are smooth maps such that rank h⁰(x) = p, ∀x ∈ Rⁿ and where J ⊂ R is an open interval of R containig 0 , Ω an open subset of Rⁿ. Let I be an open real interval containing 0 and γ : I → h(Rⁿ) a smooth arc- length parametrized curve (k ˙γ(λ)k = 1).

This article focuses on the following problem:

Local path-following problem If y0 def= h(x0) ∈ γ(I), find sufficient conditions that guar-

1Partially supported by MURST scientific funds in the framework of a COFIN 2000 project.

2Dipartimento di Ingegneria dell’Informazione, Univer- sit`a di Parma, Parco Area delle Scienze 181A, I-43100 Parma - ITALY email: [email protected]

3Dipartimento di Ingegneria Civile, Universit`a di Parma, Parco Area delle Scienze 181A, I-43100 Parma - ITALY email: [email protected]

antee the existence of a control u and ² > 0 such that the smooth solution of problem 1 verifies the following properties

y(t) ∈ γ(I) , ∀t ∈ [0, ²] (2)

and

˙y(t)^T˙γ(λ(t)) > 0, ∀t ∈ [0, ²], (3)

where λ(t) =R_t

0k ˙y(τ )kdτ .

In other words, given a path γ(I), if y0 stays on γ(I) at the initial time, we want to find crite- ria which will insure the existence of a control u which drives the output y of system (1) along the path in the same direction of the tangent to γ, at least for a while. Related problems have been considered in many papers with different general- ity and techniques (see for instance [1], [2], [3], [4], [5], [6]). In this article, we find sufficient conditions (see theorem 1) which implies a positive answer to the local path following problem, by transforming it in a constrained control problem on a manifold. Clearly we are not able to give the control u in closed form due to the gen- erality of the problem, anyway, if we specialize it to the time-varying Chaplygin systems, then conditions (5), (6) and (7), see below, reduce to the very easy criterion (15) stated in theorem 3 where a dynamic inversion generator is exhibited. The same result was found in paper [6], theorem 1, through another technique for a less general class of systems of this kind. We recall that the Chaplygin-type nonholonomic systems are an important class of mechanical systems (see for instance [7], [8]) which includes the kinematic model of various vehicles for planar and aerial navigation. Remark that, for sake of simplic- ity, we treated just time-varying nonlinear affine systems, but more general ones can be dealt by means of this technique.

The developed theory has been applied to the kinematic model of planar and aerial vehicles.

(2)

2 The main result

To state the main theorem we need some notations.

Set Γ = h⁻¹(γ(I))^def= {x ∈ Rⁿ|h(x) = γ(I)}, Γ is a smooth manifold of dimension n − p + 1 and x0 ∈Γ, the interior part of Γ. Let T^◦ x0Γ denote the tangent space to Γ at x₀ and g(0, x₀)(R^m) be the subspace of Rⁿ spanned by the columns of g(0, x0).

The following theorem gives an answer to the local path following problem.

Theorem 1 Consider system (1) in the previous hypotheses and notations . If x0 and u0are such that

y0= γ(0) (4)

f (0, x0) + g(0, x0)u0∈ Tx0Γ (5) g(0, x0)(R^m) + Tx0Γ = Rⁿ (6)

(dh dx

T

(x0) ˙γ(0))^T(f (0, x0) + g(0, x0)u0) > 0 (7) then there exist ² > 0 and one and only one smooth map u : [0, ²] → R^m such that u(0) = u0

and the map y, given by system (1), satisfies prop- erties (2) and (3).

The proof of this theorem is based on the following one.

Theorem 2 Let J ⊂ R be an open interval of R, Ω an open subset of Rⁿ, U an open subset of R^m, F : J × Ω × U → Rⁿ a smooth map, Γ ⊂ Rⁿ a smooth manifold of dimension k, with 1 ≤ k ≤ n.

If t0, x0, u0 are such that x0∈Γ and^◦

F (t0, x0, u0) ∈ Tx₀Γ (8)

∂uF (t0, x0, u0)(R^m) + Tx0Γ = Rⁿ, (9) then there exist ² > 0, one smooth couple (x, u) such x : [t0, t0+ ²] → Ω, u : [t0, t0+ ²] → U and,

∀t ∈ [t0, t0+ ²]:





˙x = F (t, x(t), u(t)) x(t) ∈ Γ

x(0) = x0, u(0) = u0.

(10)

Proof of Theorem 2

Let Λ be the open subset of Rⁿ⁺¹given by J × Ω, Σ ⊂ Λ be the product manifold J × Λ and

F : Λ × U → Rⁿ⁻¹ be the map defined by

F((τ, x), u) = (1, F (τ, x, u))^T.

Since, by (8) and (9):

F((0, x0), u0) ∈ R × Tx0Γ = T(0,x0)Σ

∂uF((0, x0), u0) + T_(0,x₀₎Σ = Rⁿ⁺¹, by corollary 1 of [9], there exists an ² > 0 and a control u ∈ C¹([0, ²], U] such that









d(τ,x)

dt = F((τ, x), u), ∀t ∈ [0, ²]

τ (0) = 0 x(0) = x0

(τ (t), x(t)) ∈ Σ, ∀t ∈ [0, ²].

which implies (10).

Proof of Theorem 1

Let F : J × Ω × R^m → Rⁿ be defined by F (t, x, u) = f (t, x) + g(t, x)u. F satisfies (8) and (9) since (5) and (6) hold; therefore we can apply theorem 2 to this F with Γ = h⁻¹(γ(I)) and U = R^m−1. Then there exist ² > 0 and a map u such that the state solution of the sys- tem (1) verifies the condition x(t) ∈ Γ, ∀t ∈ [0, ²], which implies that y(t) ∈ γ(I), ∀t ∈ [0, ²]. Finally, by (7), function ˙γ(λ(t))^T˙y(t) = ((^dh_dx(x(t))^T˙γ(λ(t)))^T˙x(t) is positive, unless of de- creasing ², therefore property (3) holds ¤.

2.1 An application: the Chaplygin-type nonholonomic systems

In this section we apply theorem 2 to solve the local path-following problem for a class of Chap- lygin systems, a class that contains, for instance, the second example presented at the end of this paper.

Consider the following time-dependent Chaplygin

system 









˙ξ = φ(t, η) + ψ(t, η)u

˙η = χ(t, η)u ξ(0) = ξ0

η(0) = η0

(11)

where φ : J × Λ → R^p, ψi : J × Λ → R^p, i = 1, . . . , p − 1, χ : J × R^p−1 → R^p−1 are smooth functions, p ≥ 2, ψ the matrix (ψ1, . . . , ψp−1), Λ

(3)

an open subset of R^p−1 and η0∈ Λ. Furthermore we suppose that ∀t ∈ R, ∀η ∈ R^p−1

ψ1(t, η), . . . , ψp−1(t, η) are orthonormal vectors (12) and

φ(t, η)^Tψ^⊥(t, η) > 0, (13) where ψ^⊥ is a unitary vector which is orthogonal to all the ψ1, . . . , ψi−1; then the following theorem holds.

Theorem 3 Let γ : I → R^p be a smooth arc- length parametrized curve and suppose that ξ0and η0 verify the following properties

ξ0= γ(0) (14)

(ψ^⊥(0, η0))^T˙γ(0) > 0; (15) then there exist ² and a unique smooth control u such that the smooth solution ξ of system (11) satisfies the following properties: ∀t ∈ [0, ²]

ξ(t) ∈ γ(I) and ˙γ(λ(t))^T ˙ξ(t) > 0, (16) where λ(t) =R_t

0k ˙ξ(τ )kdτ .

Furthermore the control u is uniquely determined by the following relation.

u = ψ(t, η)^T µ

˙γ(λ)ψ^⊥(t, η)^Tφ(t, η)

ψ^⊥(t, η)^T˙γ(λ) − φ(t, η)

¶

(17) where λ and η verify the following system:

( ˙η = χ(t, η)u , η(0) = η0

˙λ = ^ψ_ψ^⊥⊥^(t,η)(t,η)^T^T^φ(t,η)˙γ(λ), λ(0) = 0. (18)

Proof. To apply theorem 1, set n = 2p − 1 , m = p − 1, Ω = R^p× Λ,

x = µ ξ

η

¶

, f (t, x) =

µ φ(t, η) 0

¶ ,

g(t, x) =

µ ψ(t, η) χ(t, η)

¶

, h(x) = ξ ;

clearly rank h⁰(x) = p , ∀x ∈ Rⁿ and γ(I) ⊂ R^p= h(Ω).

Following the notation of theorem 1, Γ = h⁻¹(γ(I))) = γ(I) × Λ is a manifold of dimen- sion p (= (2p − 1) − p + 1). Set x0 =

µ ξ0

η0

¶ , then y0= h(x0) = ξ0= γ(0) by (14).

Furthermore

Tx2Γ = {( ˙γ(0)λ1, λ2, . . . , λp)|λj ∈ R, j = 1, . . . , p}, therefore

g(0, x0)(R^p−1) + Tx0Γ = R^2p−1⇔

⇔ ˙γ(0) /∈ ψ(0, η₀)(R^p−1) ⇔ ψ^⊥(0, η₀)^T˙γ(0) 6= 0,

then property (6) of theorem 1 holds by (15).

Moreover the hypothesis (7) of theorem 1 holds since

µ φ(0, η0) 0

¶ +

µ ψ(0, η0)u0

χ(0, η0)u0

¶

=







˙γ(0)λ1

λ1

... λp







if

u0= ψ(0, η0)^T µ

˙γ(0)ψ^⊥(0, η0)^Tφ(0, η0)

ψ^⊥(0, η0)^T˙γ(0) − φ(0, η0)

¶ ,

λ1=ψ^⊥(0, η0)^Tφ(0, η0) ψ^⊥(0, η0)^T˙γ(0) and



 λ2

... λp



 = χ(0, η0)u0.

To verify the hypothesis (7) of theorem 1 we remark that

˙γ(0)^T(dq

dx(x0)(f (0, x0) + g(0, x0)u0) =

= ˙γ(0)^T(φ(0, η0) + ψ(0, η0)u0) =

= ˙γ(0)^T( ˙γ(0)ψ^⊥(0, η0)^Tφ(0, η0) ψ^⊥(0, η0)^K˙γ(0) =

=ψ^⊥(0, η0)^Tφ(0, η0) ψ^⊥(1, η0)^T˙γ(0) > 0

by hypotheses (15) and (13).

Then if we apply theorem 1, there exists ² > 9, a control u ∈ C^∞([0, ²], R^p−1) and a couple (ξ, η) ∈ C^∞([0, ²], R^2p−1) which solves the system (11) and verifies the properties: ∀t ∈ [0, ²]

ξ(t) = h(ξ(t), η(t)) ∈ γ(I) and ˙γ(λ(t))^T ˙ξ(t) > 0.

Finally, to verify (17) and (18), we remark that if ξ, η, u are the solutions of (11) such that (16)

(4)

y(t)

v u

Γ

z

θ

Figure 1: Truck with arm.

holds, we must have:

˙γ ˙λ = ˙λ¡

ψ^⊥(ψ^⊥)^T˙γ + ψ(ψ^T˙γ)¢

=

= ˙λ

µ(ψ^⊥)^T˙γ

(ψ^⊥)^Tφψ^⊥(ψ^⊥)^Tφ + ψ(ψ^T˙γ)

¶

=

= ˙λ

µ(ψ^⊥)^T˙γ

(ψ^⊥)^Tφ(φ − ψ(ψ^Tφ)) + ψ(ψ^T˙γ)

¶

=

= ˙λ(ψ^⊥)^T˙γ (ψ^⊥)^Tφφ + ψ

µ

ψ^T( ˙λ ˙γ − ˙λ(ψ^⊥)^T˙γ (ψ^⊥)^Tφφ)

¶

which must be equal to ˙ξ = φ+ψu, since γ(λ) = ξ on [0, ²]. Therefore (17) and system (18) hold ¤.

3 Examples 3.1 Truck with arm

Consider a truck which moves along a straight railway with the speed given by a smooth function v(t, z), where z is its position along the railway. A rotating arm, of unitary length, is linked to the truck and its rotating speed is ω(t, z, θ)u where ω(t, z, θ) is a positive smooth function and θ is the angle between the arm and the railway (see figure 1). If

µ z θ

¶

is the state vector and y is the reference point coordinates, the dynamic is described by the system









 µ ˙z

˙θ

¶

=

µ v(t, z) 0

¶ +

µ 0

ω(t, z, θ)

¶ u y =

µ z + cos θ sin θ

¶

z(0) = z0, θ(0) = θ0, u(0) = u0

which is of type (1) if

x = µ z

θ

¶

, f (t, z) =

µ v(t, z) 0

¶ ,

g(t, x) =

µ 0

ω(t, z, θ)

¶

, h(x) =

µ z + cosθ sin θ

¶ ,

n = 2 , m = 1 , Ω = R×] −π 2, π

2[ , U = R;

remark that det h⁰(x) = cos θ 6= 0, ∀x ∈ U. Let I be an open real internal containing the origin, γ : I → h(Ω) = R×] − 1, 1[, γ(λ) = (γ1(λ), γ2(λ))^T, be a smooth arc-length parametrized curve and Γ = h⁻¹(γ(I)). Γ is a smooth manifold of dimen- sion 1, in fact it may be parametrized by the law µ(λ) = h⁻¹(γ(λ)) and T_µ(λ)Γ = {r ˙µ(λ) : r ∈ R}

where ˙µ(λ) =



 ˙γ1+√^γ²^˙γ²

1−γ²₂

˙γ2

√1−γ₂²



 . Remark that:

g(t, µ(λ))(R) + T_µ(λ)Γ =

=

½ r1

µ 0 ω

¶

+ r2˙µ(λ) : r1, r2∈ R

¾

which is equal to R² if and only if

˙γ1+pγ0˙γ9

1 − γ₂² 6= 0,

since ω(t, 0) 6= 0, ∀(t, θ) ∈ R×] − ^π₂,^π₂[, by hypothesis.

Therefore, by theorem 1, we can deduce the following result:

Let z0, θ0, u0 be such that y0= γ(0), and (5), (7) hold. If

˙γ0(0) + γ2(0)

p1 − γ₂²(0)˙γ2(0) 6= 0,

(notice that this means that the arm is not orthog- onal to ˙γ(0)), then there exists a unique control u, such that u(0) = u₀ and the point Q can fol- low the path γ(I), at least for a while, in the same direction of the parametrization

3.2 The Aircraft

Let the motion of the center of gravity P(t) of an aircraft be given by the following equations:

P(t) = v(t)˙



 cos θ1(t) cos θ2(t) sin θ1(t) cos θ2(t)

sin θ2(t)





where v(t) is a given smooth positive function and θ1(t) , θ2(t) are the angular polar coordinates of

(5)

θ1

θ2

θ1

θ2

Figure 2: Aircraft simplified model with the point Q.

P(t). Choose a point Q(t) rigidly linked to P(t), that is, kQ(t) − P(t)k = d > 0, and the angular polar coordinates of the vector Q(t) − P(t) are given by θ1(t) + ¯θ1 , θ2(t) + ¯θ2 where ¯θ1 and ¯θ2

are two constants such that:

−π

2 < ¯θ1< π

2 and −π

2 < ¯θ2< π 2 , (see Figure 2). Therefore Q(t) is given by:

Q(t) = P(t)+d



 cos(θ1(t) + ¯θ1) cos(θ2(t) + ¯θ2) sin(θ1(t) + ¯θ1) cos(θ2(t) + ¯θ2)

sin(θ2(t) + ¯θ2)



 ,

and its motion is governed by the following equations:

Q(t) = v(t)˙ 0

@ cos θ1(t) cos θ2(t) sin θ1(t) cos θ2(t)

sin θ2(t) 1 A +

d 0

@ − sin(θ1(t) + ¯θ1) cos(θ2(t) + ¯θ2) − cos(θ1(t) + ¯θ1) sin(θ2(t) + ¯θ2) cos(θ1(t) + ¯θ1) cos(θ2(t) + ¯θ2) − sin(θ1(t) + ¯θ1) sin(θ2(t) + ¯θ2)

0 cos(θ2(t) + ¯θ2)

1 A ·

·

θ1(t)˙ θ2(t)˙

.

Now set θ = (θ1, θ2)^T and make the change of variable

u = A(θ) ˙θ , where A(θ) =

µ d cos(θ2+ ¯θ2) 0

0 d

¶ ,

then the previous systems becomes:









 Q = v˙



 cos θ1cos θ2

sin θ1cos θ2

sin θ2



 +



 − sin(θ1+ ¯θ1) − cos(θ1+ ¯θ1) sin(θ2+ ¯θ2) cos(θ1+ ¯θ1) − sin(θ1+ ¯θ1) sin(θ2+ ¯θ2)

0 cos(θ2+ ¯θ2))



 u

˙θ = A⁻¹(θ)u.

In other words, it is the Chaplygin system:

½ ˙ξ = φ(t, η) + ψ(η)u

˙η = χ(η)u ,

if we set ξ(t) = Q(t), η(t) = θ(t) and where Λ = R× ]^π₂ − ¯θ2,^π₂− ¯θ2[,

φ : R × Λ → R³, ψ : Λ → R^3×2, χ : Λ → R² are maps defined in the obvious way. Remark that (ψ^⊥)^T = _kQ−Pk^Q−P then (12) and (13) are sat- isfied, being the columns of ψ orthonormals and

φ^Tψ^⊥= v(t) cos ¯θ1cos ¯θ2> 0,

since v(t) > 0 and −^π₂ < ¯θ1< ^π₂ , −^π₂ < ¯θ2< ^π₂. Therefore, by theorem 3, we can deduce the following result:

If P(0) and θ(0) are such that Q(0) = γ(0) and (Q(0)−P(0))^⊥˙γ(0) > 0 then there exists a unique control, given by formulas (17) and (18), such that the point Q can follow the path γ(I), at least for a while, in the same direction of the parametrization.

4 Conclusions

The problem of following a given path in an n-dimensional space by a point whose motion is governed by a time-varying non linear control affine system, has been proposed. The provided theorem gives a geometrical sufficient condition for a local solution which becomes very easy to apply in the case of the Chaplygin systems where a formula for a control dynamic inversion generator is given.

References

[1] P.S. Krishnaprasad and R. Yang, “Geomet- ric phases, anholonomy, and optimal movement,”

in Proceedings of the IEEE International Confer- ence on Robotocs and Automation, Sacramento, California, 1991, pp. 2185–2189.

[2] R. Mukherjee and D.P. Anderson, “A sur- face integral approach to the motion planning of nonholonomic systems,” Journal of Dynamic Sys- tems, Measurement, and Control, vol. 116, pp.

315–324, 1994.

[3] M. Reyhanoglu, “A general nonholonomic motion planning strategy for caplygin systems,”

in Proceedings of the IEEE Conference on Deci- sion and Control, Lake Buena Vista, FL, Decem- ber 1994, pp. 2964–2966.

(6)

[4] L. Consolini, A. Piazzi, and M. Tosques,

“Motion planning for steering car-like vehicles,”

in Proceedings of the European Control Confer- ence, Porto, Portugal, September 2001, pp. 1834–

1839.

[5] L. Consolini, A. Piazzi, and M. Tosques,

“A dynamic inversion based controller for path following of car-like vehicles,” in Proceedings of the XV IFAC World Congress, Barcelona, Spain, July 2002.

[6] L. Consolini, M. Tosques, and A. Piazzi,

“Dynamic path inversion for a class of nonlinear systems,” in Proceedings of the IEEE 2002 Con- ference on Decision and Control, Las Vegas, U.

S., December 2002, pp. 3831–3836.

[7] A.M. Bloch, M. Reyhanoglu, and N.H. Mc- Clamroch, “Control and stabilization of nonholo- nomic dynamic systems,” IEEE Transactions on Automatic Control, vol. 37, no. 11, pp. 1746–1757, November 1992.

[8] I. Kolmanovsky and N.H. McClamroch,

“Developments in nonholonomic control prob- lems,” IEEE Control Systems Magazine, vol. 15, no. 6, pp. 21–36, December 1995.

[9] L. Consolini and M.Tosques, “A sufficient condition for locally controlled invariance of a manifold for general non linear systems,” in IEEE 2003 Control and Decision Conference, submit- ted.