ANNEXE C Modle mathmatique du robot lego NXT

tir de la notice NXTway-GS Model-Based Design - Control of self-balancing two-wheeled robot built with LEGO Mindstorms NXT, Yorihisa Yamamoto.

3 NXTway-GS Modeling
This chapter describes mathematical model and motion equations of NXTway-GS.

3.1 Two-Wheeled Inverted Pendulum Model

NXTway-GS can be considered as a two wheeled inverted pendulum model shown in Figure 3-1.
W D

l
R

Figure 3-1 Two-wheeled inverted pendulum

Figure 3-2 shows side view and plane view of the two wheeled inverted pendulum. The coordinate system used in 3.2 Motion Equations of Two-Wheeled Inverted Pendulum is described in Figure 3-2.

zb
L=H 2

M , J

yl yb

l, r

ym yr

l,r

zm
m, J w R

xl

xm xb xr

: body pitch angle

l ,r
Figure 3-2

: wheel angle ( l , r indicates left and right)

l ,r

: DC motor angle

Side view and plane view of two-wheeled inverted pendulum

-6-

Physical parameters of NXTway-GS are the following.

g = 9.81
[ m / sec 2 ]

: : :

Gravity acceleration Wheel weight Wheel radius Wheel inertia moment Body weight Body width Body depth Body height Distance of the center of mass from the wheel axle Body pitch inertia moment Body yaw inertia moment DC motor inertia moment DC motor resistance DC motor back EMF constant DC motor torque constant Gear ratio Friction coefficient between body and DC motor Friction coefficient between wheel and floor.

m = 0.03 R = 0.04

[ kg ] [m]
2
2

J W = mR 2
M = 0 .6 W = 0.14 D = 0.04 H = 0.144
L=H 2

[ kgm ]

: : : : : :

[ kg ] [m] [m] [m]

[m]
2 2 2
2 2

J = ML 3 J = M W + D

) 12

[ kgm ] [ kgm ] [ kgm 2 ]

: : : : : : : : :

J m = 110 5
R m = 6.69 K b = 0.468 K t = 0.317

[ ]
[V sec rad ] [ Nm A]

n =1
f m = 0.0022 fW = 0

We use the values described in reference [2] for R m , K b , K t . We use the values that seems to be appropriate for J m , n, f m , f W , because it is difficult to measure.

3.2 Motion Equations of Two-Wheeled Inverted Pendulum

We can derive motion equations of two-wheeled inverted pendulum by the Lagrangian method based on the coordinate system in Figure 3-2. If the direction of two-wheeled inverted pendulum is x-axis positive direction at t = 0 , each coordinates are given as the following.

1 ( , ) = ( l + r ) 2

R ( r l ) W
m

(3.1)

(x m ,

ym ,

zm ) =

( x&

dt ,

& y

dt , R ,

&m , (x

& cos & m ) = R y

& sin R

(3.2) (3.3)

(xl , (x r ,

W y l , z l ) = x m sin , 2 W y r , z r ) = x m + sin , 2

ym +

W cos , z m 2 W cos , z m 2

ym

(3.4) (3.5)

( xb ,

yb , z b ) = (x m + L sin cos ,

y m + L sin sin , z m + L cos )

-7-

The translational kinetic energy T1 , the rotational kinetic energy T2 , the potential energy U are

T1 = T2 =

1 1 1 &l 2 + y &l 2 + z &r 2 + y &r 2 + z &b 2 + y &b 2 + z &l 2 + m x &r 2 + M x &b 2 mx 2 2 2

(3.6)

1 1 &2 1 2 1 &2 +1J &2 1J & & & 2 + J & ) 2 + n 2 J m ( & )2 J w + n J m ( l w r + l r 2 2 2 2 2 2

(3.7) (3.8)

U = mgzl + mgzr + Mgzb

The fifth and sixth term in T2 are rotation kinetic energy of an armature in left and right DC motor. The Lagrangian L has the following expression.

L = T1 + T2 U
We use the following variables as the generalized coordinates.

(3.9)

: Average angle of left and right wheel : Body pitch angle : Body yaw angle

Lagrange equations are the following

d L L = F & dt
d L L = F & dt
d L L = F & dt
We derive the following equations by evaluating Eqs. (3.10) - (3.12).

(3.10)

(3.11) (3.12)

&& MLR & sin = F [(2m + M )R + 2 J + 2n J ]&& + (MLR cos 2n J ) & & & sin cos = F && MgL sin ML (MLR cos 2n J ) + (ML + J + 2n J )
2 2 2 2 w m m

(3.13) (3.14) (3.15)

1 && W2 2 & sin cos = F & + + mW J J w + n 2 J m + ML2 sin 2 + 2 ML2 2 2 2 R

-8-

In consideration of DC motor torque and viscous friction, the generalized forces are given as the following

(F
r

, F , F ) = Fl + Fr , F , & ) f & & Fl = nK t il + f m ( l w l & ) f & & F = nK i + f (

t r m r w r

W (Fr Fl ) 2R

(3.16) (3.17) (3.18) (3.19)

& ) f ( &) & & F = nK t il nK t ir f m ( l m r

where il , r is the DC motor current.

We cannot use the DC motor current directly in order to control it because it is based on PWM (voltage) control. Therefore, we evaluate the relation between current il , r and voltage v l , r using DC motor equation. If the friction inside the motor is negligible, the DC motor equation is generally as follows

& )R i &l ,r = vl ,r + K b ( & Lm i l ,r m l ,r

(3.20)

Here we consider that the motor inductance is negligible and is approximated as zero. Therefore the current is

il , r =

& ) & vl ,r + K b ( l ,r Rm

(3.21)

From Eq.(3.21), the generalized force can be expressed using the motor voltage.

& + 2 & F = (v l + v r ) 2( + f w ) & & F = (v + v ) + 2 2

l r

(3.22) (3.23) (3.24) (3.25)

F =

W W2 ( + f w )& (v r v l ) 2R 2R 2

nK t nK t K b , = + fm Rm Rm

-9-

3.3 State Equations of Two-Wheeled Inverted Pendulum

We can derive state equations based on modern control theory by linearizing motion equations at a balance point of NXTway-GS. It means that we consider the limit 0 ( sin , cos 1 ) and neglect the
& 2 . The motion equations (3.13) (3.15) are approximated as the following second order term like

&& = F [(2m + M )R + 2 J + 2n J ]&& + (MLR 2n J ) & & && MgL = F (MLR 2n J ) + (ML + J + 2n J )
2 2 2 w m m
2 2 m

(3.26) (3.27) (3.28)

1 W2 2 mW J + + J w + n2 Jm 2 2 R 2

&& ) = F

Eq. (3.26) and Eq. (3.27) has and , Eq. (3.28) has only. These equations can be expressed in the form

&& & v E + F + G = H l && & v r

(3.29)

(2m + M )R 2 + 2 J w + 2n 2 J m E= MLR 2n 2 J m + f w F = 2 0 0 G= 0 MgL H =

MLR 2n 2 J m ML2 + J + 2n 2 J m

&& + J & = K (v v ) I r l
1 W mW 2 + J + J w + n2 Jm 2 2R 2 W2 ( + f w ) J= 2R 2 W K= 2R I=
2

(3.30)

- 10 -

Here we consider the following variables x 1 , x 2 as state, and u as input. x T indicates transpose of x .

T & T , u = [v , v ]T & , x2 = , x 1 = , , &, l r

(3.31)

Consequently, we can derive state equations of two-wheeled inverted pendulum from Eq. (3.29) and Eq. (3.30).

& 1 = A1 x 1 + B1u x & 2 = A2 x 2 + B2 u x

(3.32) (3.33)

0 0 A1 = 0 0

0 0 A1 (3,2) A1 (4,2)

1 0 A1 (3,3) A1 (4,3)

0 0 0 0 , B1 = B1 (3) B1 (3) A1 (3,4) A1 (4,4) B1 (4) B1 (4) 0 1

0 K I

(3.34)

1 0 A2 = 0 J

0 , B2 = I K I

(3.35)

A1 (3,2) = gMLE (1,2) det( E ) A1 (4,2) = gMLE (1,1) det( E ) A1 (3,3) = 2[( + f w )E (2,2) + E (1,2)] det( E )

A1 (3,4) = 2 [E (2,2) + E (1,2)] det( E ) B1 (3) = [E (2,2) + E (1,2)] det( E ) det( E ) = E (1,1) E (2,2) E (1,2) 2

A1 (4,3) = 2 [( + f w )E (1,2) + E (1,1)] det( E )

A1 (4,4) = 2 [E (1,1) + E (1,2)] det( E )

B1 (4) = [E (1,1) + E (1,2)] det( E )

- 11 -