Design and Implementation of Cascade LQR Controller for ...

1 2016 35information technologiesand control

Design and Implementation of CascadeLQR Controller for Stabilizationof Two-Wheeled RobotKey Words: Two-wheeled robot control; LQR control; embeddedsystems.

Ts. Slavov, J. Kralev, P. Petkov

Abstract. In this paper the developed two-wheeled robot and cas-cade LQR controller, Kalman filters, PI and PID controllers arepresented. The cascade LQR controller stabilizes the two-wheeledrobot in upright position. The PID controller ensures good track-ing of wheel position reference and the PI controller steers two-wheeled robot rotation around the vertical axis. A software inMATLAB®/Simulink environment intended for design and genera-tion of control code which is embedded in a Texas InstrumentsDigital Signal Controller is developed. Simulation and experimen-tal results of system performance are given that confirm theefficiency of the control system developed.

1. Introduction

Two-wheeled robots have several applications whichmake them useful both from theoretical and practical pointof view [1]. The most popular commercial product, built onthe idea of self- balancing two-wheeled robot is the Segway®

Personal Transporter (PT), produced by Segway Inc., USA[2]. Another self-balancing two-wheeled robot NXTway-GS[3], built on the basis of the LEGO® Mindstorms NXT de-veloper kit, is widely used in education. Also, thetelepresence and video conferencing two-wheeled robotDouble® [4] is very popular recently. The two-wheeled ro-bots have dynamics which is similar to the inverted pendu-lum dynamics so that they are inherently unstable andshould be stabilized around the vertical position using acontrol system. Linear-quadratic or proportional-integral-derivative (PID) control laws are usually implemented inorder to achieve vertical stabilization and desired positionin the horizontal plane [5].

In this paper we present the design and experimenta-tion of a control system of two-wheeled robot which imple-ments a cascade discrete-time Linear-Quadratic Regulator(LQR) with Kalman filter of 17th order for stabilization in thevertical plane in presence of several noises. The reason touse such a controller for vertical stabilization is that it istuned easier than the controllers of centralized (non-cas-cade) type. The cascade controllers also have several otherfeatures which make them attractive in the control systemsdesign [6]. In respect to the robot rotation around thevertical axis a proportional-integral (PI) controller combinedwith a 2nd order Kalman filter is used which ensures suffi-cient tracking accuracy. Due to the lack of accurate analyti-cal robot model, the control system design is done by using

a model built with the aid of an identification procedure. Asoftware in MATLAB®/Simulink environment is developedfor design and generation of control code which is embed-ded in the Texas Instruments Digital Signal ControllerTMS320F28335. Results from the simulation of the closed-loop system as well as experimental results obtained duringthe real-time implementation of the designed controller aregiven. The experimental results confirm the efficiency of thetechnical solution implemented.

2. Control System Design2.1. Experimental Set-Up

A photo and kinematic diagram of the developed bythe authors two-wheeled robot are shown in figures 1 and2, respectively.

The robot is constructed from 4 vertically connectedplastic platforms. At the bottom platform are situated 2 DCmotors, together with gearboxes, wheels and magnetic en-coders on the shafts. Motors are controlled by power am-plifier qik2s12v10 receiving commands from the processor.The next platform hosts the Spectrum DigitaleZdspTMF28335 development board, which supports TexasInstruments DSP TMS320F28335. In the DSP a stabilizingcontroller for the robot and data acquisition (DAQ) systemis embedded to allow closed-loop identification. Stabiliza-tion is achieved through inertial measurement unitADIS16350 containing three orthogonal MEMS gyroscopes.This unit is mounted on the third platform. The robot ispowered by 3 cell LiPo 12V battery situated on the upper-most platform. The input signals (u1, u2) to the motors arePWM duty cycles sent over RS232 link to the power am-plifier forming PWM voltage waveforms for the motors. Theoutput signals are wheels angular rates (θ1, θ2), body tiltrate (ϕ) and body yaw rate (ψ). Real-time data acquisitionsystem is organized around wireless communication chan-nel based on Bluetooth module.

2.2. Design MethodologyThe block-diagram of the two-wheeled robot control

system is shown in figure 3. In respect to the stabilizationin upper equilibrium state the robot is described by ARMAXand BJ models [7] obtained by means of identification pro-cedures. The ARMAX model with structure parametersna = 7, nb = 7, nc = 7, nk = 3 gives the link between thecontrol signal u and the rate ϕ, while the BJ model withstructure parameters nb = 3, nf = 7, nc = 3, nd = 3, nk = 1

. .

....

.

Print ISSN: 1312-2622; Online ISSN: 2367-5357DOI: 10.1515/itc-2016-0020

1 201636 information technologiesand control

Figure 1. Two-wheeled robot in self-balancing mode

Figure 2. Robot motion in vertical and horizontal planes


Figu

re 3

. Blo

ck-d

iagr

am o

f th

e co

ntro

l sys

tem


gives the link between the rates ϕ and θ = (θ1 + θ2)/2. Thesemodels are transformed into models with input multiplicativeuncertainty which are used latter on in the closed-loop

.. ..frequency domain analysis. In the design of cascade LQRcontroller, ARMAX and BJ models for nominal parametervalues are transformed into the state space,

(1)

( 1) ( ) ( ) ( )

( ) ( ) ( )

x k A x k B u k J v k

k C x k H v kϕ ϕ ϕ ϕ ϕ ϕ

ϕ ϕ ϕ ϕϕ+ = + +

= +

(2)

( 1) ( ) ( ) ( )( ) ( ) ( )

x k A x k B k J v kk C x k H v k

θ θ θ θ θ θ

θ θ θ θ

ϕθ

+ = + += +

where

1 2 9 1 6( ) [ ( ) ( ) ... ( )] , ( ) ( ) ... ( )TTx k x k x k x k x k x k x kϕ ϕ ϕ ϕ θ θ θ⎡ ⎤= = ⎣ ⎦

are the corresponding state vectors, vϕ (k) and vθ (k) arediscrete-time white Gaussian noises with variationsDvϕ = Dvθ = 1, and Aϕ, Bϕ, Cϕ, Hϕ, Aθ, Bθ, Cθ, Hθ are matriceswith corresponding dimensions containing the model

.

. ..

.

. . . . . . .

parameters.Design of astatic LQR controller for ϕϕϕϕϕ (LQRϕϕϕϕϕ)

The controller LQRϕ from the block-diagram in figure 3under complete information about the state is described by

. .

(3) 0( ) ( ) ( ), ( 1) ( ) ( ( ) ( ))i i i i

u k K x k K x k x k x k T r k kϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ= − + + = + −

where Kϕ and Kϕι are matrices with controller parameters,. .

. .(4) .

From equations (1), (3) and (4), for the control systemin respect to ϕ one obtains the following equation:

rϕ (k) is the reference of ϕ (k) and T0 = 0.005s is thesampling interval. Introduce the extended state vector

.

(5)

( 1) ( ) ( ) ( ) ( ), ( ) ,

( ) ( ) ( ),

x k A x k B u k G r k J v k u k K x

k C x k H v kϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ

ϕ ϕ ϕ ϕϕ

+ = + + + = −

= +where

0 0 0

9 1 9 1

1 0, , , , 0 ,

0 0 i

T C T H T rA B J G C C K K K

BA Jϕ ϕ ϕ

ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕϕϕ ϕ× ×

− −= = = = = = −

In the controller design we use the quadratic performance index

(6) 1

( ) ( ) ( ) ( )T T

kJ x k Q x k u k R u kϕ ϕ ϕ ϕ ϕ

∞

== +∑ ,

4000 0, 3

0 2 TQ RC Cϕ ϕ

ϕ ϕ

⎡ ⎤= =⎢ ⎥⎢ ⎥⎣ ⎦

.

The controller matrix Kϕ , which minimizes (6), is determined by

(7) ( )T TK R B P B B P Aϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ= + ,

where the matrix Pϕ is the positive definite solution of the equation

(8) 1( ) 0T T T TA P A P A P B R B P B B P A Qϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ−− − + + = .

The control ( )u k of the system shown in figure 3 is determined by the expression

( ) [ ( ) ( )]i

Tx k x k x kϕ ϕ ϕ=


(9) ,

where is an estimate of the state vector , obtained by a Kalman filter in respect to ( )

(10) .

The coefficient of

is determined by the expression

(11) 1( 0.00001) , ,T T T T

KF e vK DC C DC D F D F J D Jϕ ϕϕ ϕ ϕ ϕ ϕ ϕ ϕ

−= + = +

where eD is the positive definite solution of the equation

(12) 1( 0.00001)T TeD D DC C DC C Dϕ ϕ ϕ ϕ

−= − + .

From equations (1), (4), (9) and (10) one obtains for the description of the control system under incomplete informationin respect to the following equation:

(13)

1 9 0

9 9

0

9 1

9 1 9 1

( 1) ( ) ( ) ( ),

( ) ( ) ( ),

1 0

,

0

00 , ,0 0

i

i iKF KF KF

x k A x k G r k J v k

k C x k H v k

T C

A B K A B K

B K K C B K A K C A B K K C B K

TG J J

ϕ ϕ ϕ

ϕ ϕ ϕ ϕ ϕ ϕ ϕ

ϕ ϕ ϕ ϕ

ϕ

ϕ ϕ ϕ ϕ ϕ ϕ

ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ

ϕ ϕ ϕ

ϕ

×

×

×

× ×

+ = + +

= +

−

= −

− − − +

= = 1 90 0 .C Cϕ ϕ ×=

Design of astatic LQR controller for θθθθθ (LQRθθθθθ)To simplify the design, the system (13) in respect to

measurable signals is approximated by the second-ordersystem

(14) 2 2 2 2

2 2

( 1) ( ) ( )

( ) ( )

x k A x k B r k

k C x kϕ ϕ ϕ ϕ ϕ

ϕ ϕϕ

+ = +

= .

. .

The system (14) is connected in serial to the model (2), so that for the plant in respect to the controller LQRq oneobtains

(15) ( 1) ( ) ( ) ( )

( ) ( ) ( )

x k A x k B r k J v k

k C x k H v kϕθ θ θ θ θ θ

θ θ θ θθ

+ = + +

= + ,

where

2 2

2

2 6 2 11 2

6 1

0 0, , , 0

0

A BA B J C C

JB C Aϕ ϕ

θ θ θ θ θϕ θθ θ

× ××

×

⎡ ⎤= = = =⎢ ⎥⎢ ⎥⎣ ⎦

.

ˆ ( )x kϕ ( )x kϕ ϕ

0ˆ ˆ( ) ( ) ( ), ( 1) ( ) ( ( ) ( ))i i i i

u k K x k K x k x k x k T r k C x kϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ= − + = + −

KFϕ

ˆ ˆ ˆ( 1) ( ) ( ) ( ( 1) ( ) ( ))KFx k A x k B u k K k C B u k C A x kϕϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕϕ+ = + + + − −

KFK ϕ KFϕ

ϕ


The output ( )r kϕ of the controller LQRθ is determined by the expression

(16) 0ˆ ˆ( ) ( ) ( ), ( 1) ( ) ( ( ) ( ))i i i i

r k K x k K x k x k x k T r k C x kϕ θ θ θ θ θ θ θ θ θ= − + = + − ,

where ˆ ( )x kθ is an estimate of the state vector ( )x kθ , which is obtained by the Kalman filter in respect to θ ( ),

iKθ and Kθ are controller parameters. The equations of KFθ are analogous to these of KFϕ written in respect to

(15). Similarly to (13), for the description of control system one obtains

(17)

1 8 0

8 8

0

8 1

8 1 8 1

( 1) ( ) ( ) ( ),

( ) ( ) ( ),

1 0,

0

00 , ,0 0

i

i iKF KF KF

x k A x k G r k J v k

k C x k H v k

T CA B K A B K

B K K C B K A K C A B K K C B K

TG J J

θ θ θ

θ θ θ θ θ θ θ

θ θ θ θ

θ

θ θ θ θ θ θ

θ θ θ θ θ θ θ θ θ θ θ θ θ

θ θ θ

θ

×

×

×

× ×

+ = + +

= +

−= −

− − − +

= = 1 8

( )

0 0 , ( ) ( )ˆ ( )

ix k

C C x k x kx k

θ

θ θ θ θ

θ

×= =

where KFKθ is the gain of KFθ . The design of the controller LQRθ is done for the quadratic performance index

(18)

1( ) ( ) ( ) ( ) ( ) ( ),

30 0, 30

0 10

i i

ТT

k

T

J x k x k Q x k x k r k R r k

Q RC C

ϕ ϕθ θ θθ θ θ θ

θ θθθ

∞

== +∑

⎡ ⎤⎢ ⎥= =⎢ ⎥⎣ ⎦

The controller parameters are determined according to equations (7) and (8) written in respect to

00 0

8 1 8 1

01, , , , 0 ,

0 0 i

T HT C T rA B J G C C K K K

B JAθθ θ

θ θ θ θ θ θ θ θ θθ θθ× ×

−−= = = = = = −

Design of digital PID regulator for controlof θθθθθ (PIDθθθθθ)

After that the robot is stabilized in upper equilibrium

state with the controllers LQRϕ and LQRθ, it is necessary totune the parameters of the controller PIDθ which manipu-lates the angle θ of the wheels rotation. The control rθ isdetermined by

,

where rθ (k) is the reference of angle , Kθn, Kθi and Kθdare coefficients of proportionality, integration and differen-tiation. Based on the requirements to the frequencyresponses of the forward-backward robot motion control

system and to the frequency responses to the controllerPIDθ, the controller coefficients are chosen asKθn = 1.5, Kθi = 0.05 and Kθd= 0.07.

0( ) ( ( ) ( )) ( ) ( ), ( 1) ( ) ( ( ) ( ))п i d

r k K r k k K I k K k I k I k T r k kθ θ θ θ θ θ θ θθ θ θ θ= − + − + = + −

KFθ

θ


Frequency domain analysis of the forward-backwardmotion control system

Based on the plant model with input multiplicativeuncertainty and on the designed controllers, a frequency

domain analysis of the closed-loop system is done. Thesingular values of the closed-loop system, output sensitiv-ity function and controller singular values used in the analy-sis are shown in figures 4-6, respectively.

Figure 4. Closed-loop system singular value

Figure 5. Output sensitivity function


Figure 7. Robust stability

Figure 6. Controller singular values


Figure 8. Position of robot wheels

Figure 9. Angle of rotation around the vertical axis


Figure 10. Control signal

Figure 11. Tilt velocity of the robot body


Clearly, the control system tracks without static errorsthe references of wheels position with frequency up to 1rad/s and the low frequency disturbances are sufficientlysuppressed.

In figure 7 we represent the structured singular value(μ) [9] of the closed-loop uncertain system correspondingto the robust stability analysis.

The analysis of the robust stability shows that thereis sufficient stability margin in the low frequency rangewhile there are allowable uncertainties in the frequency

range of 0.9 ÷ 19 rad/s, that can make the system unstable.This is well known disadvantage of the linear quadraticregulators which make them applicable to systems withrelatively small uncertainties.

Digital PI controller for steering the robot motionaround vertical axis

In respect to the motion around vertical axis the robotis described by the determined through identification trans-fer functions

where u1 and u2 are the control signals to the left and rightwheel, respectively. The control signal of the PI controller

(PIψ) is determined by

(20) 0ˆ ˆ( ) ( ( ) ( )) ( ), ( 1) ( ) ( ( ) ( ))п itu k K r k k K I k I k I k T r k kψ ψ ψ ψ ψ ψ ψψ ψ= − + + = + − ,

.

. .

where

and 2( )kθη are uncorrelated white Gaussian

noises representing the errors in measuring wheels veloci-ties with variations 1 2

9D Dθ θ= = . For KFψ one obtains

1 1 2 2

1 2

8.5867 8.7270, ,0.1328 1 0.1186 1

0.2004 0.2009,0.0119 1 0.0119 1

u uW Wp p

W Wp p

θ θ

ψθ ψθ

= =+ +

−= =+ +

(19)

(24)

(25)

1 2

1 2 0

1 2

1 0 0( 1) 0 0( ) ( ) ( ),

0( 1) 0 1 1

( ) ( ) ( ),

( ) ( ) ( ) , ( ) ( ) ( ) ( ) ,

b

т

т bТТ

k bTx k k k

A Bk

k k k

k k k k k k k

ψ ψψ ψ

ψ ψ θ θ

ψθ η

ψψ ψ ψ

θ θ θ η ν η η

×+= + +

+

= +

= =

ˆ ˆ ˆ ˆ( 1) ( ) ( ( 1) ( ) ( ) ( ))

ˆ ˆ ˆ ˆ( 1) ( ) ( ) ( ( 1) ( ) ( ) ( ))

i

п

b b KF b т

т т KF b т

k k K k k A k B k

k A k B k K k k A k B k

ψ

ψ

ψ ψ

ψ ψ ψ ψ

ψ ψ ψ ψ ψ θ

ψ ψ θ ψ ψ ψ θ

+ = + + − − −

+ = + + + − − −

1( )kθη

where ( )r kψ is the reference of angle ( )kψ , and

iKψ are coefficients of proportionality and integration. Inorder to find a compromise between the performance of

tracking ( )r kψ and the magnitude of the control signal

( )tu k , which is add to the output of forward-backward

controller, it is chosen that 2п

Kψ = and . In

expression (20) is an estimate of the angle ,which is obtained as

(21) ,

where is an estimate of the measured by gyro veloc-

ity . In the computation of control one uses the es

timate ˆ ( )kψ , taking into account that the measured bygyro velocity ψ(k) is given by

(22) ,where ψb(k) is the bias and ψm(k) is the exact rotation ratearound the vertical axis. The bias in (22) is determinedfrom [8]

(23)

,

where is a discrete-time white Gaussian noise with

variation 1D ψν = . The estimate ˆ ( )kψ is determined by PI

Kalman filter ( ). Tis filter is designed in respect to anextended model which is obtained after discretization of thetransfer functions (19) and taking into account the equa-tions (22) and (23),

пKψ

0.01i

Kψ =

ˆ ( )kψ ( )kψ

0 ˆˆ ˆ( 1) ( ) ( )k k Т kψ ψ ψ+ = +

ˆ ( )kψ

( )kψ

( ) ( ) ( )т bk k kψ ψ ψ= +

0( 1) ( ) ( )b bk k bT kψψ ψ ν+ = +

ψν

KFψ


where i п

ТKF KF KFK K K

ψ ψ ψ= is the filter gain. It is

determined according to expressions (11) and (12) written in

respect to the model (24). The estimate ˆ ( )kψ is determinedfrom

(26) .

3. Experimental Results

A simulation scheme of the control system and aspecialized software in MATLAB®/Simulink environment isdeveloped to implement the control code. With the aid ofSimulink Coder and Code Composer Studio, a code isgenerated from this software which is embeddedin the Texas Instruments Digital Signal ControllerTMS320F28335 [10]. Several experiments are performed withthe designed controllers and comparison is done with thesimulation results. The results from experiment and simula-tion concerning the basic signals of the control system areshown in figures 8-11.

It is seen from figure 8 the perfect tracking of thewheels position reference and the independence of theforward-backward motion on the robot rotation around thevertical axis. The usage of instead of ensuresexact robot rotation around the vertical axis while a signifi-cant drift in the measured value of is observed withthe time, which is due to the integration of the gyro bias(figure 9). The system control signal has an acceptablemagnitude and is significantly smaller than the magnitudeconstraints of ±50 (figure 10). The robot body tilt velocityis shown in figure 11.

4. Conclusions

The paper presents the developed cascade LQR con-troller, Kalman filters, PID and PI controllers intended tocontrol the plane motion of a two-wheeled robot. The de-veloped algorithms are embedded in the Digital Signal Pro-cessor TMS320F28335. Experimental and simulation resultsof the robot motion are given which demonstrate the per-formance of the embedded control system. The closenessbetween the experimental and simulation results shows theefficiency of the developed control system. Better results inrespect to the robustness of the closed-loop system may beachieved by using H∞ or μ-controllers.

References

1. Campion, G., W. Chung. Wheeled Robots. In B. Siciliano and O.Khatib, Editors, Springer Handbook of Robotics, Chapter 17,Springer, Berlin, 2008, 391-410.2. Segway Personal Transporters. Bedford, NH, 2012.http://www.segway.com.

3. Yamamoto, Y. NXTway-GS (Self-Balanced Two-Wheeled RobotController Design). http://www.mathworks.com/matlabcentral/fileexchange/ .4. Double Robotics. Sunnyvale, CA, 2014.http://www.doublerobotics.com .5. Chan, R. P. M., K. A. Stol, C. R. Halkyard. Review of Modellingand Control of Two-wheeled Robots.m – Annual Reviews in Con-trol, 37, 2013, 89-103.6. Goodwin, G. C., S. F. Graebe, M. E. Salgado. Control SystemDesign, Ch. 10. Prentice Hall, Upper Saddle River, NJ, 2001,ISBN: 9780139586538.7. Ljung, L. System Identification: Theory for the User, 2nd Ed.Prentice-Hall, Inc., Englewood Cliffs, NJ, 1999, ISBN: 978-0136566953.8. Slavov, Ts., P. Petkov. Strapdown Inertial System Based onImproved MEMS Error Models. – Cybernetics and InformationTechnologies, 11, 2011, 3-23.9. Zhou, K., J. C. Doyle, K. Glover. Robust and Optimal Control.Prentice Hall, Upper Saddle River, NJ, 1996, ISBN:0-13-456567-3.10. Spectrum Digital, Inc. 2007. eZdspTMF28335 Technical Ref-erence. http://c2000.spectrumdigital.com/ezf28335/docs/ezdspf28335c_techref.pdf .

( )kψ

( )kψ

ˆ ( )kψ

ˆ ˆ( ) ( ) ( )bk k kψ ψ ψ= −


Manuscript received on 15.04.2016

Tsonyo Slavov was born in Sofia,Bulgaria in 1978. He graduated fromthe Department of Systems and Con-trol of the Technical University of Sofiain 2002. He received Ph.D degree in2007. Presently he is an AssociatedProfessor in the Department of Sys-tems and Control at the TechnicalUniversity of Sofia.

Jordan Kralev has M.S. degree inAutomatics, Information and ControlTechnologies. Since 03.2013 he is aPh.D. student in the Department ofSystems and Control, Technical Uni-versity of Sofia. His work is in thefield of implementation of complexcontrol structures with programmableplatforms.

Petko Petkov received M.S. andPh.D. degrees in control engineer-ing from the Technical University ofSofia in 1971 and 1979. Since 1995,he has been a Professor of ControlTheory in the Department of Sys-tems and Control at the TechnicalUniversity of Sofia. He is coauthor-ing Computational Methods for Lin-ear Control Systems (Prentice Hall,Hemel Hempstead, U.K., 1991),Perturbation Theory for MatrixEquations (North-Holland,Amsterdam, 2003), and Robust Con-

trol Design with MATLAB (Springer, London, 2005, 2013).Contacts:

Department of Systems and ControlTechnical University of Sofia

1756 Sofia, BulgariaCorresponding author, e-mail: [email protected]

Design and Implementation of Cascade LQR Controller for ...

Documents

Transcript of Design and Implementation of Cascade LQR Controller for ...