Trajectory Tracking Control of Nonholonomic Mechanical Systems in Presence of Model Uncertainties

(1)

18^th Annual International Conference on Mechanical Engineering-ISME2010 11-13 May, 2010, Sharif University of Technology, Tehran, Iran

1

ISME2010-XXXX

Trajectory Tracking Control of Nonholonomic Mechanical Systems in Presence of Model Uncertainties

Khoshnam Shojaei, Ahmadreza Tarakameh, Alireza Mohammad Shahri Mechatronic and Robotic Research Laboratory,

Electronic Research Center, Deparatment of Electrical Engineering;

Iran University of Science and Technology, Tahran, IRAN {shojaei | a.tarakameh}@ee.iust.ac.ir, [email protected]

Abstract

This paper presents a robust adaptive feedback linearizing control law to solve the integrated kinematic and dynamic trajectory tracking problem of nonholonomic mechanical systems in presence of parametric and nonparametric uncertainties. An adaptive nonlinear control law is proposed based on input-output feedback linearization technique to get asymptotically exact cancellation of the parametric uncertainty in the system parameters. Then, a leakage modification is applied to modify integral action of the adaptation law to compensate for the nonparametric uncertainties due to friction and unmodeled dynamics.

Simulation results are presented to illustrate the robustness and tracking performance of the proposed controller.

Keywords: Feedback linearization, Parametric uncertainty, Robust adaptive, Nonholonomic system.

Introduction

Trajectory tracking control of mechanical systems subject to nonholonomic constraints has been an attractive research area in mechanical engineering over past decades. Typical examples of such systems are manipulators, space robots and wheeled mobile robots [1] with nonholomomic constraints [5]. The trajectory tracking problem is concerned with the design of a controller to force a nonholonomic mobile system to track a geometric path with an associated timing law [4]. A variety of control algorithms is developed to solve this problem in the literature [4], [7], [9], [10].

Since models of nonholonomic systems are highly nonlinear, the feedback linearization technique offers a qualified candidate to design tracking control laws.

There are many works that propose tracking controllers based on feedback linearization for such systems [2], [9], but they use exact kinematic and dynamic models.

Considering that Feedback linearization is based on cancellation of nonlinear terms, this cancellation may not be achieved perfectly in presence of unknown kinematic and dynamic models. This fact motivated us to study and design a robust adaptive feedback linearizing control law to solve the trajectory tracking problem of nonholomic mechanical systems in presence of model uncertainties.

The main contribution and novelty of the present work lies in designing an adaptive input-output feedback linearizing controller to solve the integrated kinematic and dynamic trajectory tracking problem of nonholonomic systems. A leakage modification is applied to the parameter update rule to avoid parameters drift due to the nonparametric uncertainties such as friction and unmodeled dynamics. After a brief review of the kinematic and dynamic model of nonholonomic systems, the tracking controller is proposed based on SPR-Lyapunov design approach. Then, the adaptive tracking controller is modified to be robust against nonparametric uncertainties and simulation results are presented for a nonholonomic wheeled mobile robot (WMR) to illustrate the robustness and tracking performance of the proposed controller.

Model of Nonholonomic Mechanical Systems

Consider the general formulation of a mechanical system subject to m nonholonomic constraints:

( ) ( , ) ( ) ( ). ( )^T

M q q C q q q d t+ + =B q τ−A q λ (1)

( ) 0

A q q = (2)

where q = [q1, q2,...,qn]^T denotes a vector of generalized coordinates, τ is (n-m)-vector of actuators inputs, M(q) is a n×n symmetric positive-definite inertia matrix,

( , )

C q q is a n×n centripetal and Coriolis matrix. The vector d(t) denotes unmodeled dynamics, coulomb and viscous friction. B(q) is the input transformation matrix.

Equation (2) denotes Pfaffian form of the nonholonomic constraints and A(q) is also a full rank matrix [2].

Assume that S(q) = [s1(q), s2(q),..., sn-m(q)]^T is a full-rank matrix which is made up of a set of smooth and linearly independent vector fields in the null space of A(q), i.e.

A(q). S(q) = 0 (3)

According to (2) and (3), it is possible to write the kinematic equation of the system motion in terms of an auxiliary (n-m)-vector time function v(t) which is called pseudo-velocities vector as

( ). ( )

q =S q v t (4)

where v(t) = [v1(t), v2(t),..., vn-m(t)]^T. Differentiating (4)

(2)

2 ISME2010, 11-13 May, 2010 yields q =S q v( ). +S q v( ). which is substituted in (1)

and the result is multiplied by S^T(q) to give the following dynamic equation:

1 1 1 1

1 1

( ) ( ) ( ) ,

, ( ) ,

,

T T T

T T

M v t C q v t d B

M S MS C q S MS S CS B S B d S d

τ

+ + =

= = +

= =

(5)

Assume that the mechanical system is driven by electrical actuators such as DC motors. According to [11], the electrical equation of a DC motor may be represented as follows by ignoring the inductance of the motor:

1 _a 2 ,

k u k Xv

τ = − (6)

where k1 and k2 are related to motor constants. The matrix X is a transformation matrix which transforms motors shaft velocities to pseudo-velocities vector. ua is (n-m)-vector of voltage input of the actuators. By substituting (6) in (5) and considering (4), the kinematic and dynamic equations of motion of a nonholonomic mechanical system can be represented as:

2

2 2 2 2

2 2 1 2

2 1

( ). ( ),

( ) ( ) ( ) ,

, , ,

( ) ,

T T T

T T

a

q S q v t

M v t C q v t d B u M S MS B k S B d S d C q S MS S CS k B X

=

+ + =

= = =

= + +

(7)

These equations of motion may be integrated into the following state space representation in companion form:

1

2 2

1 1

2 2 2 2

( ) ( , ) ( , ) ( ),

( )

( , ) ( )

0 ( ) , ( , )

0

0 0

,

x f x q x g x ua x

f x

g x x

s q v

q x M C v

M B M d

θ θ ξ

θ ξ

θ ₋

− −

= + + +

=

= =

= −

−

⎡ ⎤

⎢ ⎥

⎣ ⎦ ⎣ ⎦

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦

(8)

where x = [q^T,v^T]^T is the state vector. This representation allows us to apply the differential geometric control theory for trajectory tracking problem.

Assumption 1: Measurements of all states, i.e. x = [q^T,v^T]^T, are available in real-time.

Assumption 2: Pseudo-velocities of the mechanical system, i.e. v(t) = [v1(t), v2(t),..., vn-m(t)]^T, are bounded for all time t > 0.

Controller Design

A trajectory tracking control law can be designed based on robust adaptive feedback linearization technique for non-holonomic system given in (8). One may check that the system (8) is not input-sate linearizable. Thus, an input-output feedback linearization is taken into

account. For the control design purposes, following output equation must be considered:

( ) [ ( ),1 , _{n m}( )]^T

y =h x = h q … h₋ q (9)

Definition 1: Given a smooth bounded reference trajectory yr(t) which is generated by a reference system, then the integrated kinematic and dynamic tracking control problem is to design an output feedback control for the system (8) such that it satisfies:

lim( ( ) ( )) 0

t y t y tr

→∞ − = ⁽10)

The basic approach to obtain a linear input-output relation is to repeatedly differentiate the outputs so that they are explicitly related to inputs. After differentiating, we obtain:

2

. ( ). ( ).

( ) ( )

( ) ( ).

f q a

f q f

f g f a

g h

y u

J q S q v

L h L h L h L h

y L h x L L h x L L h x L L h x u

ξ

= +

=

+ +

= +

+ +

(11)

where L L h x_g _f ( ) D x( ) is the decoupling matrix:

1 2

1

( )

n m n m n m

g f g f

D x

L L h L L h

L L h L L h

− − −

=

⎡ ⎤

⎢ ⎥

⎣ ⎦

…

(12)

Main Theorem: Provided that the reference trajectory yr(t) is selected to be bounded for all times t > 0, and under assumptions 1 and 2, the following tracking controller guarantees that the tracking error e(t)= y(t)- yr(t) and parameter estimation error are uniformly ultimately bounded:

1

1 2

1

( ).( 2 ),

( ) ( ),

ˆ ˆ

a ˆ

T

f q f

r r r

u x

y y y y y

W E

D η L h L L h

η β β

θ θ

= − −

= + − + −

= Γ − ΓΣ

− ⁽¹³⁾

where " ^ " denotes the estimated value of a vector, W is (n-m)×p regression matrix, E1 is a vector of filtered error signals and Г is a p×p symmetric and positive definite matrix as the adaptive gain. β1 and β2 are diagonal matrices which denote derivative and proportional gains of the linear control law for the entire system, respectively. The vector η is a new external control input. Σ = σIp×p is a positive-definite diagonal matrix.

Proof: Considering the certainty equivalence principle in adaptive control systems, the following nonlinear feedback is chosen to get asymptotically exact

(3)

3 ISME2010, 11-13 May, 2010 cancellation of the parametric uncertainty in the system

parameters:

1( ).( 2 ).

ˆ

a f q f

u =D⁻ x η−L h L L h− ⁽¹⁴⁾ where

ˆ ˆ

ˆ ( ) _g _f

( ),

_q _f _q _f

D x =

L L h x

L L h =L L h ⁽¹⁵⁾

By substituting (15) in (11), we have:

2

1 2

( ) ( ) ( )

( ). ˆ ( ).( ).

f q f f

f q f

y L h x L L h x L L h x D x D x L h L L h

ξ

η

−

= + +

+ − −

(16)

After some manipulation, equation (16) may easily be written in the following form:

1 2

( ) ( ).( )

( ) ( )

ˆ

q f f

f q f

x x

y L L h x L L h x

D D L h L L h

η ξ

η

−

= + +

+ − −

(17)

where

( ) ( ) ( )

( ) ( ) ( ),

ˆ

q f q f q f

x x x

L L h x L L h x L L h x D =D −D

= − ⁽¹⁸⁾

Then, the parametric model from of (17) may be achieved as:

. _f ( )

y =η+W θ+L L h x_ξ ⁽¹⁹⁾ where θ θ θ= − ˆ represents the vector of parameter estimation errors. Now, the adaptive law may be derived by SPR-Lyapunov design approach which is motivated from Sastry [6], [12] and Ioannou [13]. Assume that the external control input ηj for j-th subsystem of (19) is chosen such that j-th output, yj(t), tracks the desired output, yjr(t):

1 2

1, 2, ,

( ) ( ) ,

j jr j jr j j jr j

j n m

y y y y y

η β β

= −

= + − + −

…

(20)

This yields the following error equation:

1 2 ( )

j j j j j j f

e +β e +β e =W θ+L L h x_ξ ⁽²¹⁾ where Wj is the j-th row of regression matrix. For purposes of adaptation, one may use the following filtered error signal for j-th output:

j ej jej

ε = +α ⁽²²⁾

Since the derivative of ej is known as a function of measured states by considering (11), it is obvious that εj

is available. The parameter αj is chosen such that the following transfer function is strictly positive real:

2

1 2

( ) ^j

j

j j

T s s

s s

α

β β

= +

+ +

(23)

This means that Tj(s) is analytic in the closed right half plane and Re[Tj(jω)] > 0. Accordingly, by positive real lemma [13], there exist the positive definite matrices Pj

and Qj such that

T ,

j j j j j

T

j j j

A P P A Q

P B C

+ = −

=

(24)

where matrices Aj, Bj and Cj are defined by minimal state space realization of (21) and (22) in the following form:

( ( ))

j j j j j

j j j

f j

X A X B W

C X

L L h x_ξ θ

ε

= +

=

+ ⁽²⁵⁾

where X _j =[ ,e e_j _j]^T is the state variable and

2 1

0 1 0

, , [ ,1]

j j 1 j j

j j

A B C α

β β

= = =

− −

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥ ⎣ ⎦

⎣ ⎦

(26)

As a result, the entire system error equation may be written as:

1

( _f ( ))

X AX B W E CX

L L h x_ξ θ

= +

=

+ ⁽²⁷⁾

where

1 2

( , , , ),

[ , , , ]

n m

T T T T

n m

A diag A A A B diag B B B C diag C C C

X X X X

−

=

…

(28)

The Lyapunov equation (24) is also written for entire system as follows:

,

.

T T

A P PA Q PB C

+ = −

=

(29)

where

1 2

( , , , ),

n m

P diag P P P Q diag Q Q Q

−

=

…

(30)

(4)

4 ISME2010, 11-13 May, 2010 Now, one may define the following Lyapunov function

to derive adaptive law.

1 1 1

( , )

2 2

T T

V X θ = X PX + θ Γ⁻θ (31)

By taking time derivative of the proposed function and applying (27) and (29), we may write:

1

1 1

( , ) ( )

2

( ( ))

T T T

T f

V X X QX W E

L L h x_ξ E

θ = − +θ + Γ⁻θ +

(32)

Considering that θ = −θˆ and replacing the adaptive law from (13) in (32) and ˆθ θ θ= − , we obtain:

1

1 2

( ( ))

T T T

T f

V X QX

L L h x_ξ E

θ θ θ θ

= − + Σ − Σ

+

(33)

Consider the minimum singular values of matrices Q and Σ, i.e.

min( ^T ), min( ^T ),

Q Q Q

µ = λ µ_Σ = λ Σ Σ (34)

and L L h x_ξ _f ( ) ≤δ , we have

2

1

( , ) 1 || || . || ||

2 ^Q

T

V X X

E

θ µ µ θ θ

µ θ δ

Σ

≤ − +

− +

(35)

Consider the following fact:

2 2 2

2

1 1

| || . || || || || || || ,

2 2

|

θ θ θ κ θ κ

κ

≤ + ∈ ℜ+ (36)

Besides, one may write 1 ² ₁

2

T

A − µQ X +δ E and

2

2 2 2

2

2 2 2

1 2

1 1

4

1 2

( )

4

1 1

4

T Q

Q

A X C X

X C

µ δ

µ

µ δ

µ

µ δ

µ

≤ − +

− −

≤ − +

(37)

Inequalities (36) and (37) may help write (35) as follows:

2

2 2 2

2

2 2

2

2 2 2 2

1 1

( , ) 4

1 1

( || || || || )

2 2

1 1

(1 )

4 2

1 1

|| ||

2

Q

V X X C

X C

θ µ µ θ δ

µ

µ θ κ θ

κ

µ µ θ

κ

δ µ κ θ

µ

Σ

≤ − − +

+ +

≤ − − −

+ +

(38)

By defining the following parameters,

1 2 2

2 2 2 2

1 1

0, (1 ) 0,

4 2

1 1

|| || 0 2

Q

C

ν µ ν µ

κ

ρ δ µ κ θ

µ

Σ

= > = − >

= + >

(39)

Inequality (38) may be written as

2 2

1 2

( , )

V X θ ≤ −ν X −ν θ +ρ (40)

On the other hand, the Lyapunov function in (31) may be stated as:

2 1 2

max max

( , ) ( ) ( )

V X θ ≤λ P X +λ Γ⁻ θ (41)

where λmax(M) represents the maximum eigenvalue of M. It follows for

1 2

1

max max

min , ,

( ) ( )

k P

ν ν

λ λ ⁻

= Γ

⎧ ⎫

⎨ ⎬

⎩ ⎭

(42)

equation (40) becomes

( , ) ( , ) .

V X θ ≤ −kV X θ +ρ (43) After solving the differential inequality (43), we have

( ) (0) ^kt (1 ^kt), [0, ).

V t V e e t

k

− ρ −

≤ + − ∀ ∈ ∞ (44)

We can utilize (31) to write that

2 1 2

min min

( ) ( ) , ( ) ( ) .

V t ≥λ P X V t ≥λ Γ⁻ θ (45) Noting (43), V is upper bounded by ρ/k which together with (45) results in:

2 2

1

min min

, .

( ) ( )

X k P k

ρ ρ

λ θ λ ⁻

≤ ≤

Γ

(46)

Since ρ is bounded, equation (43) implies that the error vector X and parameter estimation error are uniformly

(5)

5 ISME2010, 11-13 May, 2010 ultimately bounded and robustness of the adaptive law

is achieved in presence of the uncertainty δ. □ A Design Example

In this section, the proposed control law is used to solve the trajectory tracking problem of a nonholonomic differential drive wheeled mobile robot. The configuration of a differential drive WMR is shown in Fig. 1. The WMR have two conventional fixed wheels on a single common axle and a castor wheel to maintain the equilibrium of the robot. The centre of mass of the robot is located in PC. The point P0 is the origin of the local coordinate frame that is attached to the WMR body and is located at a distance d from PC. The point PL is a virtual reference point on x axis of the local frame at a distance L (look-ahead distance) of P0. The parameter 2b is the distance between two fixed wheels.

The radius of each wheel is denoted by r. If one choose the generalized coordinates vector of the WMR system as q = [xO, yO, φ]^T, one velocity constraint is obtained as follows:

cos sin 0

O O

y ϕ−x ϕ = (47)

Thus, we define pseudo-velocities as v = [v, ω]^T which are linear and angular velocities of the WMR body.

According to the notation introduced before, the following kinematic and dynamic matrices are obtained:

2 2

2 2 2

2

cos 0

( ) sin 0 , 0 ,

0 1 0

2 1

, 1

2 ( ) ( ). ( ) ( ).

,

.

C

d

S q M m

I

K b

r m d r r

C X

b K b

m d r r r

d t B q F q B q ϕ

ϕ

τ

= =

− −

= +

⎡ ⎤

⎢ ⎥

⎢ ⎥ ⎣ ⎦

⎢ ⎥

⎣ ⎦

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎣ ⎦

⎣ ⎦

(48)

where d(t) includes the friction vector, F, and unmodeled dynamic, τd. The following output variables are chosen to track a desired trajectory based on Look- ahead control method (n-m = 2):

1 2

( ) [ ( ), ( )]

[ cos , sin ]

T

O O

y h x h q h q

x L ϕ y L ϕ

= =

= + +

(49)

By defining the parametric vector as:

2

2 2

1 1

[ , m^C , b , , b]^T

mr m Ir mr Ir

θ = (50)

and following (14)-(19), the parametric model is easily achieved. For more details, the interested reader is referred to our previous work [11].

A computer simulation is performed to evaluate the performance of the proposed controller. In this

simulation, parameters are chosen to match with a real world mobile robot, and Gaussian white noise is also added to the states to simulate a localization system. In order to show the performance of the proposed tracking controller in this paper, the proposed controller in [11]

is also considered for comparison. A bounded smooth desired trajectory is chosen as follows:

1 2

( ) sin(2 ),

( ) sin( ),

r g r

y t x R t

y t y R t

ω ω

= +

(51)

where (xg , yg) and R are the centre and radius of circular trajectory, respectively and ωr = 0.05 rad/sec. This simulation is carried out using Euler method with the time step of 0.02 s. In order to provide a smooth navigation, a critically damped system is chosen by setting β2j= β1j2 /4 in error equation (21). Following parameters are selected for simulation: r = 0.15 m, b = 0.75 m, d = 0.3 m, L = 0.1 m, mw = 1 Kg, mC = 36 Kg, Iw = 0.005 Kg.m², Im = 0.0025 Kg.m², IC = 15.625 Kg.m², sampling time dt = 0.02, R = 7.5 m, (xg , yg) = (10m, 25m), k1 = 7.2, k2 = 2.6. To illustrate the robustness of the proposed controller, the uncertain values of parameters, friction upper bound constants and disturbance are selected as:

1 2 3 4 4

ˆ(0) [0.1 , 0.05 , 0.3 , 2 2 ] ( ) 0.9 0.75,

[10 sin(3 ), 10 sin(3 )]

, ^T,

T d

F q q

t t

θ θ θ θ θ θ

τ

=

≤ +

=

(52)

In addition, the control signals for actuators are saturated to lie within |ua| < 24v. The mobile robot starts from x(0) = [2.5, 6, -π/6, 0, 0]^Tto track the generated trajectory by (51). Fig. 2 shows the desired trajectory and trajectory of the WMR for two controllers. The position tracking errors are also illustrated in Fig. 3.

These figures show the robustness and performance of trajectory tracking controllers. The adaptive tracking controller with sigma-modification on adaptation law shows more robustness to disturbance and friction, while the performance of the adaptive control law without modification is degraded in presence of unstructured uncertainties.

Figure 1: Configuration of a nonholonomic differential drive WMR.

(6)

6 ISME2010, 11-13 May, 2010

0 1 2 3 4 5

3 4 5 6 7 8

X(meter)

Y(meter)

0 20 40 60 80 100 120

-1 0 1

Time(sec.)

e1 (m)

AFL Robust AFL

0 20 40 60 80 100 120 140

-0.5 0 0.5

Time(sec.)

e2 (m)

Figure 2: Desired and WMR trajectories for both controllers in x-y plane: adaptive controller with modification (red sold line) and adaptive controller without modification (blue dashed line).

Figure 3: Tracking errors for both controllers: adaptive controller with modification (red sold line) and adaptive controller without modification (blue dashed line).

Conclusion

A trajectory tracking controller has been designed based on feedback linearization technique for nonholonomic mechanical systems. The proposed controller has been applied to a nonholonomic WMR. Simulation results shows that the proposed control law is effective in presence both parametric and nonparametric uncertainties.

References

[1] G. Campion, G. Bastin, and B. D. Andrea-Novel,

“Structural Properties and Classification of Kinematic and Dynamic Models of Wheeled Mobile Robots". IEEE Transactions on Robotics and Automation, vol. 12, no. 1, pp. 47-62, 1996.

[2] N. Sarkar, X. Yun, and V. Kumar, “Control of mechanical systems with rolling constraint:

Application to dynamic control of mobile robots,”

Int. J. Robot. Res. , vol 13, no. 1, pp. 55-69, 1994.

[3] P. Coelho and U. Nunes, “ Path-following control of mobile robots in presence of uncertainties,”

IEEE Transactions on Robotics, vol. 21, no. 2, April 2005.

[4] J. P. Pedro Aguiar, “Trajectory-Tracking and Path-Following of Underactuated Autonomous Vehicles With Parametric Modeling Uncertainty,”

IEEE Tranactions on Automatic Control, vol. 52, no. 8, pp. 1362-1379, 2007.

[5] I. Kolmanovsky, H. McClamroch,

“Developments in Nonholonomic Control Problems,” IEEE Control Syst. Mag. , pp. 20-36, Dec. 1995.

[6] S. S. Sastry, A. Isidori, “Adaptive Control of Linearizable Systems,” IEEE Trans. Automatic Control, AC-34, pp. 1123-1131, 1989.

[7] Y. Yamamoto, X. Yun, “Coordinating Locomotion and manipulation of a Mobile Manipulator,” Recent trends in mobile robots, World Scientific Series in Robotics and Automated Systems, vol. 11, pp. 157-181, 1993.

[8] B. D. Andrea-Novel, G. Bastin, G. Campion,

“Dynamic Feedback Linearization of Nonholonomic Wheeled Mobile Robots".

Proceedings of the International Conference on Robotics and Automation, pp. 2527-2532, 1992.

[9] G. Oriolo, A. De Luca, M. Vendittelli “WMR Control Via Dynamic Feedback Linearization:

Design, Implementation, and Experimental Validation,” IEEE Transactions on Control Systems Technology, vol. 10, no. 6, pp. 835-852, 2002.

[10] W. E. Dixon, D. M. Dawson, “Tracking and Regulation Control of a Mobile Robot System with Kinematic Disturbances: a Variable Structure-Like Approach,” Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control , vol. 122, pp. 616-623, 2000.

[11] Kh. Shojaei, A. Tarakameh, A. Mohammad Shahri, , “Adaptive trajectory tracking of WMRs based on feedback linearization technique,”

Proceedings of the IEEE International Conference on Mechatronics and Automation, pp. 729-734, China, 2009.

[12] S. S. Sastry and M. Bodson, “Adaptive Control:

Stability, Convergence and Robustness,”

Englewood Cliffs, NJ: Prentice-Hall, 1989.

[13] P. A. Ioannou and J. Sun, “Robust Adaptive Control,” Prentice-Hall, Englewood Cliffs, NJ, 1996.

[14] G. Campion B. d’Andrea-Novel, G. Bastin,

“Modelling and state feedback control of nonholonomic mechanical system,” Proceedings of the 30^th Conference on Decision and Control, IEEE, pp. 1184-1189, England, 1991.