Gain Scheduling Control Experiment ofBalancing Transformer Robot using LEGO Mindstorms

Kentaro Hirata, Mayumi Tomida, and Kazuyoshi HatadaGraduate School of Information Science,Nara Institute of Science and Technology

Ikoma, Nara, 630-0192, Japan

ABSTRACT

In this paper, we consider a Hands-On experiment with LEGOMindstorms to demonstrate advanced control theory, especiallythe gain scheduling method. Such a learning style is extensivelystudied in the field of engineering education these days. Our goalis to create a mobile robot which transforms its posture whilemaintaining the balance. First, we derive the equation of motionof our robot. Since the inertia matrix contains the scheduling pa-rameter related to the posture, we remove it through a redundantdescriptor expression. Based on the obtained LPV (Linear Pa-rameter Varying) model in LFT (Linear Fractional Transforma-tion) form, we design gain scheduling controllers via the dilatedLMIs (Linear Matrix Inequalities). Finally, we show the simula-tion results and experiments.

keywords: Gain Scheduling, LPV System, Dilated LMI and De-scriptor Form

1 INTRODUCTION

To prevent the recent tendency among young generations to moveaway from the scientific field and to fill the gap between the the-ory and practice in engineering discipline, growing attention isfocused on hands-on education [1]. In this paper, we describeour attempt to introduce this hands-on approach into the study ofadvanced theories in the control engineering discipline. Specifi-cally, advanced control experiment using LEGO Mindstorms [2]is considered1. As is widely understood, Mindstorms is a suitabletool for hands-on experiences on robotics. Actually, our institutehas been providing kids robot school (Fig. 1) using Mindstormsfor local community and elementary schools more than five yearsas one of our Academic Volunteer Education activities. It is usednot only in elementary education [4], but also in control experi-

Fig. 1 Kids Robot School

1LEGO and Mindstorms are trademarks of the LEGO Group.

ments for undergraduates [9, 7], in Lab experiments [3] and evenin introductory education for engineers in industries [8].

From control engineering perspective, various attempts includ-ing anti-sway control experiment [9] (Fig. 2) based on classicaland modern control theory, or stabilization of balancing robot(NXTway-GS) [10] (Fig. 3) based on modern and robust controltheory are reported.

Fig. 2 Anti-sway experiment

Fig. 3 NXTway-GS

Our target here is a demonstration of the gain scheduling controlon Mindstorms platform. This control scheme is classified as oneof the advanced robust control theory. In general, the discrepancybetween the theory and practice becomes larger as the theoreticaladvance goes further. This is why we selected the gain schedulingcontrol. Fig. 4 shows our LEGO based robot and the target be-havior. Such an acrobatic motion with LEGO robot is appealingand can emphasize the importance of the control theory behindthe modern high-tech products even for newcomers. Usually, theconstruction of laboratory-level control experimental facilities byourselves costs time and money. In contrast, the experiment hereonly requires with one LEGO Mindstorms kit, one gyro sensor(Fig. 5) sold by a third party company [6] and a laptop PC.

Fig. 4 Balancing Transformer Robot

Fig. 5 Gyro Sensor

The software development environment nxtOSEK [5] with realtime OS for LEGO Mindstorms is provided as a free and opensource software. Another potential merit to use LEGO Mind-storms is that we can avoid the time-consuming system identifi-cation process inevitable for usual lab experiment with custom-made plants. Since LEGO Mindstorms is produced under its in-dustrial standard, the dynamics of the robots made from the sameparts are exactly the same.

The following notations are used. For a square matrixA,He[A] = A+AT . Symmetric part in LMI condition is expressedby ∗.

2 MODELING

Balancing Transformer RobotOur robot maintains the balance by driving two wheels on theground. The motor installed at ”waist” is only used for trans-forming. This is a kind of two-link under-actuated robot. How-ever, its dynamical behavior is not the same as famous relativeslike Pendubot [11] or Acrobot [12]. Thus we need to derive itsmathematical model first.

Fig. 6 shows a schematic diagram of our robot. The variablesθw,θb andϕ denote the wheel rotation angle, the lower link rotationangle and the angle between the upper and lower links (postureangle), respectively in [rad]. This plant can be regarded as a pa-rameter dependent system in terms of the posture angle. The con-trol input is the voltage command for the DC motor denoted byv. The model parameters are summarized in Table 1.

For simplicity, let us assume uniform mass distributions for bothlinks. Then the moment of inertia of the whole body around the

Fig. 6 Schematic diagram of our robot

Table 1 Parameters of our robot

M1 = 0.565 [kg] upper link weightM2 = 0.170 [kg] lower link weightm = 0.030 [kg] weight of a wheell1 = 0.170 [m] upper link lengthl2 = 0.255 [m] lower link lengthr = 0.040 [m] wheel diameter

g = 9.807 [m/s2] gravity accelerationRm = 6.690 [Ω] internal resistance

Kb = 0.468 [V· s/rad] DC motor speed constantKτ = 0.317[Nm/A] DC motor torque constant

Im = 1× 10−5[kg・m2] rotor moment of inertiacm =0.0022[Nm· s/rad] viscous friction coefficient

rotation axis of the wheels is given by

Ib(ϕ) =M1l

21 +M2l

22

12+

M1M2(l21 + l22)/2 + l1l2 cosϕ2(M1 +M2)

.

The moment of inertia of one wheel is given byIw = mr2/2.Using the following notations

Mℓ =

(1

2M1 +M2

)l1,

α1 = (M1 +M2 + 2m)r2 + 2(Iw + Im),

α2(θb, ϕ) = rMℓ cos θb +1

2rM2l2 cos(ϕ− θb),

α3(ϕ) =M2

ℓ +M22 l

22/4 +MℓM2l2 cosϕ

M1 +M2+ Ib(ϕ),

α4 = 2

(KτKb

Rm+ cm

)α5(θb, θb, ϕ) = −rMℓ sin θbθ

2b +

r

2M2l2 sin(ϕ− θb)θ

2b ,

α6(θb, ϕ) = −gMℓ sin θb +1

2gM2l2 sin(ϕ− θb),

α7 = 2Kτ/Rm

θ =[θw θb

]T,

Lagrange’s equation of motion of the robot is given by[α1 α2(θb, ϕ)

α2(θb, ϕ) α3(ϕ)

]θ + α4

[1 −1−1 1

]θ

+

[α5(θb, θb, ϕ)α6(θb, ϕ)

]= α7

[1−1

]v. (1)

LPV Representation in Descriptor Form

For controller design, we need linearized model of (1) aroundthe equilibrium point determined by the posture angleϕ. Let θbdenote the lower link rotation angle corresponding to the equi-librium state. From the balancing condition that the center ofgravity of the link system is located on a vertical line crossingthe rotational center of the wheels, explicite expression ofθb as afunction ofϕ can be derived as

θb(ϕ) = arctanM2l2sinϕ

2Mℓl1 +M2l2 cosϕ. (2)

The change ofθb versusϕ is depicted in Fig. 7. If we restrict

Fig. 7 Relationship betweenθb andϕ

the transforming range asϕ ∈ [0, π/2], the least square ap-proximation of θb(ϕ) in this interval is given byθb(ϕ) = kϕ,k = 0.1726. Let ∆θb denote the deviation from the equilib-rium, i.e., θb = θb + ∆θb. Based on this, we linearize thenonlinear equation of motion (1). We regardθb as a small an-gle since the range corresponding to the intervalϕ ∈ [0, π/2]is θb ∈ [0, 0.2745]. Taylor expansion ofcos θb aroundθb = 0yieldscos θb ≃ 1− θ2b/2. By choosingρ := θ2b as the schedulingparameter, nonlinear functions in (1) can be expressed as

cos θb = 1− ρ

2, cosϕ = 1− ρ

2k2,

cos(ϕ− θb) = 1− ρ

2

(1

k− 1

)2

.

By substituting these parameterizations into (1), we obtain thefollowing LPV system representation in the descriptor form:[

I 00 E1 + ρE2

] [θ

θ

]=

[0 I

A1 + ρA2 A3

] [θ

θ

]+

[0B2

]v,

(3)

where

e1(1) = rMℓ +1

2rM2l2,

e1(2) =(Mℓ +M2l2/2)

2 +M1M2(l1 + l2)2/4

M1 +M2

+1

12(M1l

21 +M2l

22),

e2(1) = −1

2rMℓ −

1

4rM2l2

(1

k− 1

)2

,

e2(2) = −M2l1l22k2

,

E1 =

[α1 e1(1)

e1(1) e1(2)

], E2 =

[0 e2(1)

e2(1) e2(2)

],

a1 = Mℓg +1

2M2gl2,

a2 = −1

2Mℓg − 1

4M2gl2

(1

k− 1

)2

,

A1 =

[0 00 a1

], A2 =

[0 00 a2

],

A3 = α4

[1 −1−1 1

], B2 = α7

[1−1

],

θ =[θw ∆θb

]T.

Conversion into LFT Form

Since the coefficient matrix in the left-hand side of (3) also con-tains the scheduling parameterρ, it is difficult to apply existingLMI-based analysis and synthesis techniques directly. To over-come this situation, the redundancy of the representation of thedescriptor form can be used [12]. Specifically,ρ-dependent termsare moved to the left-hand side by adding an algebraic constraint.Then we derive an LPV model in LFT form by eliminating theredundant state as shown below.

Multiplication of E−11 from the left to the second row of (3)

yields

θ = A1θ + A3θ + ρ(A2θ − E2θ) + B2v, (4)

where

A1 = E−11 A1, A2 = E−1

1 A2, A3 = E−11 A3,

E2 = E−11 E2, B2 = E−1

1 B2.

Let ξ1 = θ, ξ2 = θ. Introduce the third descriptor variable

ξ3 = A2θ − E2θ,

which corresponds to an algebraic constraint. Then (4) is writtenas

ξ2 = A1ξ1 + A3ξ2 + ρξ3 + B2v.

By denoting

ξ =[ξ1 ξ2 ξ3

]T, E =

[I 00 0

],

A0 =

[0 IA1 A3

], AL =

[0−I

], AR =

[E2A1 − A2 E2A3

],

B0 =

[0B2

], BR = E2B2, D = −E2,

A(ρ) =

[A0 ρAL

AR I − ρD

], B =

[B0

BR

],

one can derive a redundant descriptor system equivalent to (3) as

Eξ = A(ρ)ξ + Bv.

By eliminatingξ3, an LPV model in LFT form is given as

θa(t) = A(ρ)θa(t) +B(ρ)v(t), (5)

θa =[ξ1 ξ2

]T,

where[A(ρ) B(ρ)

]=

[A0 B0

]+ALρ(I−ρD)−1 [ AR BR

].

3 GAIN SCHEDULING CONTROLLER DESIGN

Modern gain scheduling is a scheme to use variable controllerstructures when the value of the time-varying parameter of anLPV system can utilized online [13].

Dilated LMI Approach

Here we consider the problem of designing a gain schedulingstate feedback law

v(t) = K(ρ)θa(t), (6)

against the plant (5). We use dilated LMI approach [14], [15] todecouple the parameter-dependent variables to reduce the compu-tational burden. In [12], the authors are designing a gain schedul-ing controller for Acrobot by combining anH∞ specification anda constraint on the pole assignment region. We consider here anLQ-type specification with pole assignment since it is better fortuning based on the trial designs for frozen systems.

Lemma 1 (Dilation Lemma [14], [15]) Let the matricesA11 ∈Rn×m, A12 ∈ Rn×l, A21 ∈ Rl×m, A22 ∈ Rl×l and a sym-metric matrixP ∈ Rn×n are given. Suppose thatA22 is non-singular. Then, the following two conditions are equivalent.

(i) There existsX ∈ Rm×n satisfying theLMI

P +He(A11 +A12A−122 A21)X < 0.

(ii) There existX ∈ Rm×n, V ∈ Rl×nandW ∈ Rl×l satisfyingtheLMI[P +He[A11X ] ∗

−A21X 0

]+He

[[A12

A22

] [V W

]]< 0.

(7)

Potential merits of using a dilated LMI condition like (7) are

• No cross term betweenX andA12 orA22 appears.

• It does not contain the inverse ofA22.

LQ-type Design via Dilated LMI

Let us consider the performance index

J =

∫ ∞

0

[θa(t)TQθa(t) + v(t)TRv(t)]dt,

with Q = HTH ≥ 0 andR > 0 for the plant (5) and the initialconditionθa(0) = θ0a. When there existγ(ρ) > 0, X(ρ) andF (ρ) satisfying the following LMI He[A(ρ)X(ρ) +B(ρ)F (ρ)] ∗ ∗

HX(ρ) −I ∗−F (ρ) 0 −R−1

< 0, (8)

[γ(ρ)I ∗I X(ρ)

]> 0, (9)

then the state feedback (6) with the gain

K(ρ) = F (ρ)X−1(ρ), (10)

stabilizes the closed-loop system and the performance criterion

J < γ(ρ)∥θ0a∥2

is satisfied [16]. With

[AO

11 AO12(ρ)

AO21 AO

22(ρ)

]=

A0 0 B0 ALρH 0 0 00 0 −I 0

AR 0 BR I − ρD

,

XO(ρ) =

X(ρ) 0 00 0 0

F (ρ) 0 0

,

PO =

0 ∗ ∗0 −I ∗0 0 −R−1

,

the LMI (8) is written as

PO +He(AO11 +AO

12AO−122 AO

21)XO < 0. (11)

From the dilation lemma, this is equivalent to[PO +He[AO

11XO] ∗−AO

21XO 0

]+He

[[AO

12

AO22

] [V W

]]< 0.

(12)

Thus our design problem is now reduced to a feasibility problemto find XO, FO, V, W satisfying (12) and (9) simultaneously.We restrict the variablesX(ρ) andF (ρ) to be affine inρ whereasV andW are supposed to be constant. Then the whole condition(12) becomes affine inρ. Consequently, (12) can be expressedby a convex combination of the conditions corresponding to themaximum and the minimum values ofρ. If one can find the solu-tions at two endpoints,X(ρ) andF (ρ) can also be obtained froma convex combination of these endpoint solutions.

Pole Assignment Region Constraint via Dilated LMI

The procedure to assign the closed-loop poles of (5) with the statefeedback (6) inside a prescribed circle is shown in [12]. Given acircle centered at(−q, 0) with radiusr, the desired assignment isachieved if there existX(ρ) andF (ρ) satisfying[

(q2 − r2)X(ρ) 00 −X(ρ)

]+He

[[−qAF (ρ) 0AF (ρ) 0

]]< 0,

(13)

AF (ρ) = A(ρ)X(ρ) +B(ρ)F (ρ).

Under the notations

AD11 =

[−qA0 −qB0 0 0A0 B0 0 0

],

AD12(ρ) =

[ALρ 00 ALρ

],

AD21 =

[−qAL −qBR 0 0AL BR 0 0

],

AD22(ρ) =

[I − ρD 0

0 I − ρD

],

XD(ρ) =

X(ρ) 0F (ρ) 00 X(ρ)0 F (ρ)

,

PD(ρ) =

[(q2 − r2)X(ρ) 0

0 −X(ρ)

],

the LMI condition (13) is rearranged into[PD +He[AD

11XD] ∗−AD

21XD 0

]+He

[[AD

12

AD22

] [V W

]]< 0.

(14)

Similarly to the case of (12), one can reduce this problem intotwo endpoint conditions by an adequate choice of the order of thevariables inρ.

4 SIMULATION AND EXPERIMENT

Simulation

Via the synthesis procedure given in the previous section, we de-sign a gain scheduling controller for our robot. The minimumand the maximum values of the scheduling parameterρ corre-sponding to the posture angle rangeϕ ∈ [0, π/2] areρmin = 0,ρmax = 0.0735. We use the following design parameters:

Q = diag[1× 103, 1× 103, 1, 1], R = 1× 103,

γ(ρ) = (1− η)γ1 + ηγ2, η =ρ− ρmin

ρmax − ρmin,

γ1 = 4.03× 105, γ2 = 3.22× 105,

q = 0, r = 80.

We denote the endpoint solutions by

X1 := X(ρmin), X2 := X(ρmax),

F1 := F (ρmin), F2 := F (ρmax).

Under the setting described above, we solve a feasibility problemconsisting of 6 LMIs, i.e., (9), (12) and (14) forρ = ρmin andρmax, in terms of the variablesX1, X2, F1, F2, V andW. Fromthese solutions, we determine the feedback gainK(ρ) by (10)where

X(ρ) = (1− η)X1 + ηX2, F (ρ) = (1− η)F1 + ηF2.

The simulation result of the time response of the obtained closed-loop system under transformation is shown below. The initialstate is given byθa(0) = [0, 0.1, 0, 0]T . For the first 5 seconds,the posture angleϕ is set to be 0. Then it is increasedπ/30 [rad]per second (Fig.8). The transition of the angle of the lower linkθb (the body angle) is plotted in Fig. 9. The balancing stabilityis maintained under the time-varying posture angle resulting in ashape transformation.

Fig. 8 Angle of postureϕ

Fig. 9 Angle of bodyθb

Experiment

In the experiment, we experience large spillover vibration due tothe backlash of the ”waist” gear. So we modified the structure toreduce the amount of vibration (Fig. 10).

Fig. 10 Improved Gear Structure

The second idea for the implementation is to reduce the com-plexity of the online computation. If we implement the controllaw (10) ”as is”, one must write the source code for matrix inver-sion. Due to the limited computational resource of Mindstorms,it is rather difficult to run such codes on the embedded CPU. Byusing a matrix inverse formula, one can rewriteK(ρ) as

K(ρ) =F (ρ) adj (X(ρ))

det(X(ρ)).

Since one can compute the numerator and the denominator offlineas a function ofρ by using some symbolic math softwares, thecomputation of the feedback gain (10) can be realized by simplearithmetic operation givenρ.

Fig. 11 is the snapshots of our experiment. The posture angleis changed in time fromπ/2 to 0. The left column shows theresult when the stabilizing controller forϕ = π/2 is used. Itloses stability as the posture angle apart from the designed value.The case with the gain scheduling controller is shown in the rightcolumn. In contrast to the fixed controller case, the stability ismaintained during (and after) the transformation.

5 CONCLUSIONS

In this paper, the gain scheduling control of a balancing trans-former robot made from LEGO Mindstorms is considered. Afterthe modeling and conversion into an LPV system representation,we applied the dilation approach to solve a feasibility problemin terms of the parameter dependent LMIs. The obtained designprocedure is verified via numerical simulations and experiments.

REFERENCES

[1] M. Sanpei: Miscellaneous Thoughts on Control Education-Theory, Practice, Hands-On-, Measurement and Control,Vol. 46, No. 9, pp. 681/682 (2007) (in Japanese)

[2] http://mindstorms.lego.com

[3] R. Watanabe: Control Experiment using LEGO Mind-storms, Proc. of the5th Annual Meeting SICE Control Di-vision, pp. 753/758 (2005) (in Japanese)

[4] http://www.legoeducation.jp/

mindstorms/teaching/index.html

[5] http://lejos-osek.sourceforge.net/jp/index.

htm

[6] http://www.hitechnic.com/

[7] B. S. Heck, N. S. Clements, and A. A. Ferri: A LEGO Ex-periment for Embedded Control System Design, IEEE Con-trol System Magazine, Vol. 24, Issue 10, pp. 61/64 (2004)

[8] ET Robot Contest, Japan Embedded Systems TechnologyAssociation,http://www.etrobo.jp/

[9] P J. Gawthrop, E. McGookin: A LEGO-Based Control Ex-periment, IEEE Control Systems Magazine, Vol. 24, Is-sue 10, pp. 43/56 (2004)

[10] Model Based Development of NXTway-GS, The Math-Works, Inc.

[11] H. Kajiwara, P. Apkarian, P. Gahinet: LPV Techniques forControl of an Inverted Pendulum, IEEE Control SystemsMagazine, Vol. 19, Issue 2, pp. 44/45 (1999)

[12] M. Kawata: Gain Scheduling Control System Synthesisof an Acrobot Based on Dilated LMIs, Proc. of the36thSICE Control Theory Symposium, pp. 405/410 (2007) (inJapanese)

[13] T. Iwasaki: LMI and Control, Shokodo (1997) (in Japanese)

[14] Y. Ebihara and T. Hagiwara: Control System Analysis andSynthesis Using Dilated LMIs, Systems, Control and Infor-mation Vol. 48, No.9, pp. 355/360 (2004)

[15] Y. Ebihara, T. Hagiwara: A Dilated LMI Approachto Continuous-Time Gain-Scheduled Controller Syn-thesis with Parameter-Dependent Lyapunov Variables,Trans. SICE, Vol. 39, No. 8, pp. 734/740 (2003)

[16] A. Fujimori: Robust Control, Coronasha (2001) (inJapanese)

(a) Fixed Controller (b) Gain Scheduling

Fig. 11 Snapshots of Experiment