这个作业是使用Matlab通过完成PID和模型预测控制来平衡两轮Lego机器人

ELEC6228 Coursework Design Project P1

Revision History: cf,bc1d11, 12/01/2020, version 6 Page 1 of 5

P1

Applied Control Systems: Design Project

Please read through this document and complete the

coursework as required. The coursework submitted

(including the relevant program source files) should be all

your own work and that reference to, quotation from, and

discussion of other work has been correctly acknowledged.

Note: This coursework will count towards 50% of your

final mark for this module. Please spend no more than 50

hours on this assignment.

ELEC6228 Coursework Design Project P1

Revision History: cf,bc1d11, 12/01/2020, version 6 Page 2 of 5

Applied Control Systems: Design Project

Part A – Lego Mindstorms NXT Robot

In Part A, you will work by yourself to use PID and model predictive control to balance a twowheel Lego robot (shown below) and verify the performance of your design in Simulation using

Matlab and Simulink. Please note you can use any system configuration (e.g. constructions, use

of different sensors/measurements) as long as it achieves the design objectives, but you may

want to weigh up achievability versus challenge in making your decision.

Preparation

Read about Lego Mindstorms NXT robot to have a good understanding of the platform. You

should also get yourself familiar with Matlab and Simulink. You should be able to find many

references on these topics – here are a few examples:

1. http://en.wikipedia.org/wiki/Lego_Mindstorms_NXT#NXT-G

2. http://ctms.engin.umich.edu/CTMS/index.php?aux=Home

Task A.1 Mathematical Modelling of the Robot Dynamics

In order to use different control design methods to balance the robot, a mathematic model of the

robot dynamics is needed. Read the Appendix and make sure you understand how to establish a

mathematic model of the robot dynamics.

Task A.2 Modelling of the Robot Dynamics in Simulink

Build the model obtained in Task A.1 in Matlab Simulink. For different initial conditions of the

robot, simulate the system model and observe the system behaviours. Are they consistent with

your expectations?

Task A.3 A Simple PID Controller

Design a PID controller to balance your robot. Clearly describe your design procedure and the

measurements you use. Evaluate the performance of your PID controller.

ELEC6228 Coursework Design Project P1

Revision History: cf,bc1d11, 12/01/2020, version 6 Page 3 of 5

Task A.4 Model Predictive Control

Use model predictive control to balance your robot. Clearly describe your design procedure and

the measurements you use. Evaluate the performance of your controller. How would you tune

your MPC controller in practice? Compare your MPC design with the PID design in Task A.3 –

what do you find? Can you explain why?

Task A.5 Discussion

Critically evaluate your work. Have you achieved the objectives? How would you improve your

design? What problems did you find during this work? What conclusions can you draw from

your design? From the experience of this work, what do you need to consider when solving a

practical problem?

Part B – Quanser QUBE-Servo with NI myRIO

In this part, you will work in group to apply the three control design methods, i.e. linear

quadratic control, model predictive control and iterative learning control to the Quanser QUBEServo system with NI myRIO. Note that the Quanser QUBE-Servo can be configured into either

an inertial disc (the design objective is for the output angle to follow a periodic square wave

(amplitude ��� and period � seconds) or an inverted pendulum (the design objective is to

balance the pendulum).

Please choose the configuration according to your own preference. Note you may want to weigh

up achievability versus challenge in making your decision.

Preparation

After attending the NI training session, you should now be familiar with myRIO, LabVIEW and

the Quanser QUBE. If not, please read more about the training session notes or/and talk to the

module lecturers.

Task B.1 Configuring the Quanser QUBE-Servo

Choose a configuration and connect your system appropriately. Check you configuration and

make sure the system works properly.

ELEC6228 Coursework Design Project P1

Revision History: cf,bc1d11, 12/01/2020, version 6 Page 4 of 5

Task B.2 Modelling of the Robot Dynamics

In order to control the system, a mathematical model is needed. Establish a mathematical model

of the system. Clearly specify the input and output of the model, your methods in obtaining the

model and any assumptions you have made. Is the model obtained a good description of the

physical plant? Evidence this.

Task B.3 Linear Quadratic Optimal Control

Design a linear quadratic optimal controller to achieve the design objective. Clearly describe

your design procedure. Evaluate the performance of your designed controller. How would you

tune the parameters of the controller?

Task B.4 Model Predictive Control

Use model predictive control to design the controller. Clearly describe your design procedure.

Evaluate the performance of your controller. How would you tune the MPC controller

parameters and what criteria would you choose?

Task B.5 Iterative Learning Control

Design an iterative learning control law to achieve the design objective. Clearly describe your

design procedure and any assumptions you have made. Evaluate the performance of your

controller.

Task B.6 Comparison and Discussions

Compare the performance of the three control design methods. What do you find? Can you

explain your observations?

Critically evaluate your work. Have you achieved the objectives? How would you improve your

design? What problems did you find during this work? What conclusions can you draw from

your design? From the experience of this work, what do you need to consider when solving a

practical problem?

Assessment of Your Work: The project accounts for 50% of your final mark for this module. You will

be asked to upload a report and relevant program source files (with clear instructions on how to run the

program) before the deadline (16:00 26/04/2020) using the ECS handin system.

Template of the Report: This assignment requires you to produce a detailed report of your application

of the theory to real experimental systems in the form of an EIGHT page academic journal or conference

paper, using either the Word or LaTeX template available below:

http://www.ieee.org/publications_standards/publications/authors/author_templates.html

The report should consist of two parts, each carrying equal marks:

Part A should describe your implementation of Model Predictive Control on the LEGO Mindstorm

platform. This should be written as an individual activity, and all tasks involving LEGO Mindstorm

should likewise be completed as an individual activity.

Part B is should detail the implementation of Optimal Control, Model Predictive Control and Iterative

Learning Control on the Quanser QUBE-Servo platform, and fully evaluate and compare their respective

performance. Part B should be written as a group activity, as should all tasks involving the QUBE

platform. Note that the QUBE platform can be configured as either a servo-inertia, or a servo-pendulum.

ELEC6228 Coursework Design Project P1

Revision History: cf,bc1d11, 12/01/2020, version 6 Page 5 of 5

It is up to you what choice you make, but you may want to weigh up achievability versus challenge in

making your decision.

Both Part A and Part B should provide sufficient detail of the steps taken to implement the various

controllers, and provide motivation for the decisions made. It is anticipated that additional research into

the theoretical content presented in lectures will form part of your report. Results should be thoroughly

evaluated and compared, and you should indicate the unsolved issues and problems. The report should

be prepared as if it was a journal article going forward to publication – i.e. checked for spelling and other

errors and of the highest quality you can produce. Although Part A and Part B should comprise separate

sections of the report, they should be accompanied by additional introduction, background, discussion

and conclusion sections which integrate the two parts. A possible structure is:

Section 1. Introduction and Theoretical Background

Section 2. Application to LEGO Mindstorm

Section 3. Application to Quanser QUBE

Section 4. Discussion

Section 5. Conclusions and Future Work

– 4 –

2 NXTway-GS System

This chapter describes the structure and sensors / actuators of NXTway-GS.

2.1 Structure

Figure 2-1 shows the structure of NXTway-GS. A Hitechnic gyro sensor is used to calculate body pitch angle.

HiTechnic

Gyro Sensor

Ultrasonic Sensor

Left DC Motor

Left Rotary Encoder

Right DC Motor

Right Rotary Encoder

2.2 Sensors and Actuators

Table 2-1 and Table 2-2 show sensor and actuator properties.

(N1) : The heuristic maximum sample rate for measuring accurate distance.

Sensor Output Unit Data Type Maximum Sample [1/sec]

Rotary Encoder angle deg int32 1000

Ultrasonic Sensor distance cm int32 50 (N1)

Gyro Sensor angular velocity deg/sec uint16 300

Figure 2-1 NXTway-GS

Table 2-1 Sensor Properties

Table 2-2 Actuator Properties

Actuator Input Unit Data Type Maximum Sample [1/sec]

DC Motor PWM % int8 500

– 5 –

The reference [1] illustrates many properties about DC motor. Generally speaking, sensors and actuators are

different individually. Especially, you should note that gyro offset and gyro drift have big impact on balance

control. Gyro offset is an output when a gyro sensor does not rotate, and gyro drift is time variation of gyro

offset.

– 6 –

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

H

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.

: body pitch angle l,r

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

: DC motor angle

r l

R

Figure 3-1 Two-wheeled inverted pendulum

Figure 3-2 Side view and plane view of two-wheeled inverted pendulum

l,r

z y

r

y

m

y

b

y

l

y

l

x m

x b

x r

x x

m l , r

W b

z

m

z

L H 2

M , J

w m, J

R

– 7 –

Physical parameters of NXTway-GS are the following.

g 81.9 ]sec/[ : Gravity acceleration 2 m

m 03.0 kg][ : Wheel weight

R 04.0 m][ : Wheel radius

2

2 mRJ W

][ : Wheel inertia moment 2

kgm

M 6.0 kg][ : Body weight

W 14.0 m][ : Body width

D 04.0 m][ : Body depth

H 144.0 m][ : Body height

HL 2 m][ : Distance of the center of mass from the wheel axle

3

2 MLJ ][ : Body pitch inertia moment 2

kgm

12 22 DWMJ ][ : Body yaw inertia moment 2

kgm

: DC motor inertia moment 5

J m 101 ][

2

kgm

69.6 : DC motor resistance Rm

][

468.0 Kb

radV ]sec[ : DC motor back EMF constant

Kt 317.0 ANm ][ : DC motor torque constant

n 1 : Gear ratio

f m 0022.0 : Friction coefficient between body and DC motor

0 W

f : Friction coefficient between wheel and floor.

. KKR tbm We use the values described in reference [2] for ,,

We use the values that seem to be appropriate for , because they are difficult to measure.

m Wm

,,, ffnJ

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 coordinate is given as follows.

rl lr W

R

2

1

, (3.1)

Rdtydtxzyx mmm m m

,, ,, , , cos RRyx sin mm

(3.2)

lll m m m

z

W

y

W

xzyx ,cos

2

,sin

2

,, (3.3)

rrr m m m

z

W

y

W

xzyx ,cos

2

,sin

2

,, (3.4)

,, Lxzyx ,cossin Ly ,sinsin Lz cos bbb m m m

(3.5)

,

– 8 –

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

2 2 2 2 2 2 2 2 2

1

2

1

2

1

2

1

l l l r r r b b b T m x y z m x y z M x y z (3.6)

2 2 2 2 2 2 2 2

2

( )

2

1

( )

2

1

2

1

2

1

2

1

2

1

w l w r m l m r T J J J J n J n J (3.7)

l r Mg b U mgz mgz z (3.8)

The fifth and sixth term in are rotation kinetic energy of an armature in left and right DC motor. The

Lagrangian

T2

L has the following expression.

L T1 T2 U (3.9)

We use the following variables as the generalized coordinates.

: Average angle of left and right wheel

: Body pitch angle

: Body yaw angle

Lagrange equations are the following

F

L L

dt

d

(3.10)

F

L L

dt

d

(3.11)

F

L L

dt

d

(3.12)

We derive the following equations by evaluating Eqs. (3.10) – (3.12).

2m M R 2J w

2n J m MLRcos 2n J m MLR sin F

2 2 2 2

(3.13)

MLRcos 2n J m ML J 2n J m MgL sin ML sin cos F

2 2 2 2 2

(3.14)

J n J ML ML F

R

W

mW J w m

sin 2 sin cos

2 2

1 2 2 2 2

2

2

2

(3.15)

– 9 –

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

l r Fr Fl

R

W F F F F F F

2

, , , , (3.16)

l t l m l w l F nK i f ( ) f (3.17)

r t r m r w r F nK i f ( ) f (3.18)

( ) ( )

t l t r m l m r F nK i nK i f f (3.19)

where is the DC motor current. l r

i

,

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 and voltage using DC motor equation. If

the friction inside the motor is negligible, the DC motor equation is generally as follows

l r

i

, l r

v ,

(3.20)

m l r l r b l r m l r L i v K R i

, , , ,

( )

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

m

l r b l r

l r

R

v K

i

( )

, ,

,

(3.21)

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

2 2 l r w F v v f (3.22)

F vl

vr 2 2 (3.23)

r l w

f

R

W

v v

R

W

F 2

2

2 2

(3.24)

m

m

t b

m

t

f

R

nK K

R

nK

, (3.25)

– 10 –

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

second order term like . The motion equations (3.13) – (3.15) are approximated as the following 2

m M R J w

n J m MLR n J m F

2 2 2

2 2 2 2 (3.26)

MLR n J m ML J n J m MgL F

2 2 2

2 2 (3.27)

J n J F

R

W

mW J w m

2

2

2

2

2 2

1

(3.28)

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

r

l

v

v

E F G H (3.29)

H

MgL

G

f

F

MLR n J ML J n J

m M R J n J MLR n J

E

w

m m

w m m

0

0 0

2

2 2

2 2 2 2

2 2 2

2 2 2

( )

r l

I J K v v (3.30)

R

W

K

f

R

W

J

J n J

R

W

I mW J

w

w m

2

2

2 2

1

2

2

2

2

2

2

where

where

We now represent these two equations in state-space form.

– 11 –

Here we consider the following variables as state, and as input. indicates transpose of .

1 2

x , x u

T

x x

T

l r

T T

x , , , , x , , u v , v 1 2

(3.31)

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

x1 A1x1 B1u (3.32)

x2 A2 x2 B2u (3.33)

(4) (4)

(3) (3)

0 0

0 0

,

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

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

0 0 0 1

0 0 1 0

1 1

1 1

1

1 1 1

1 1 1

1

B B

B B

B

A A A

A A A

A

(3.34)

K I K I

B

J I

A

0 0

,

0

0 1

2 2

(3.35)

2

1

1

1

1

1

1

1

1

det( ) (1,1) (2,2) (1,2)

(4) (1,1) (1,2) det( )

(3) (2,2) (1,2) det( )

(4,4) 2 (1,1) (1,2) det( )

(3,4) 2 (2,2) (1,2) det( )

(4,3) 2 (1,2) (1,1) det( )

(3,3) 2 (2,2) (1,2) det( )

(4,2) (1,1) det( )

(3,2) (1,2) det( )

E E E E

B E E E

B E E E

A E E E

A E E E

A f E E E

A f E E E

A gMLE E

A gMLE E

w

w