two wheeled balancing lego robot - it.uu.se · two wheeled balancing lego robot jonsson per, piltan...
TRANSCRIPT
Two wheeled balancing LEGO robot
Jonsson Per, Piltan Ali, Rosén Olov
September 17, 2009
Abstract
A two wheel inverted pendulum robot have been constructed using
the building bocks from LEGO mindstorms kit. A 3-DOF mathemat-
ical model describing the motion of the robot has been derived using
Lagrangian dynamics. The objective of this project were to design con-
trollers for balancing and line tracking. Two regulators were designed us-
ing the mathematical model, one PID regulator and one LQG regulator.
Software for development and simulation of the regulators were created
using Matlab and its toolbox Control Toolbox. These regulators were
then implemented and evaluated on the real system using RobotC. Both
the PID and LQG regulator successfully balanced the robot and the LQG
regulator were also able to perform servo regulation, where the reference
signals to the robot were given using a gamepad connected to a computer.
Also an algorithm for line tracking using a light sensor attached to the
front of the robot were implemented and tested. Using the algorithm the
robot was able to follow an eight shaped track with good performance.
But the algorithm could denitely be improved to make better lap times.
1
Contents
1 The robot 3
1.1 Modeling of the robot . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.2.1 Lego Mindstorms NXT . . . . . . . . . . . . . . . . . . . 8
1.2.2 Sensors and actuators . . . . . . . . . . . . . . . . . . . . 8
2 Control 10
2.1 Kalman ltering . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.1 Short about Kalman ltering . . . . . . . . . . . . . . . . 10
2.1.2 Our Kalman lter . . . . . . . . . . . . . . . . . . . . . . 11
2.2 PID regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2.1 Short about the regulator . . . . . . . . . . . . . . . . . . 11
2.2.2 Our regulator . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.2.3 Tuning of the regulator . . . . . . . . . . . . . . . . . . . 13
2.2.4 Simulation results . . . . . . . . . . . . . . . . . . . . . . 13
2.3 LQG Regulator with integral action . . . . . . . . . . . . . . . . 16
2.3.1 Short about LQG regulators . . . . . . . . . . . . . . . . 16
2.3.2 Design of LQG regulator . . . . . . . . . . . . . . . . . . . 17
2.3.3 Schematic picture of the regulated system . . . . . . . . . 18
2.3.4 Our regulator . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.3.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . 19
3 Line tracker 22
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.2 The algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4 Software 25
4.1 Lego software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.1.1 Functionality . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.1.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . 25
2
5 Appendix 28
5.1 Overview of the programming design process . . . . . . . . . . . 29
5.1.1 In Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.1.2 In RobotC . . . . . . . . . . . . . . . . . . . . . . . . . . 30
1 The robot
1.1 Modeling of the robot
When designing controllers a good mathemathical model for the robot is neededfor several reasons. For example it is much easier to test dierent controllers ona model using a simulation enivronmeant instead of trying them on the robot.In this way we can design and analyze the controller on the model before imple-mentation. Also most control design methods are model based and this meansthat the model will aect the performance of the designed controllers, thus agood model is essential. There are several articles published on the modelingof the two wheeled inverted pendulum,[1, 2, 3], where the mathematical modelis given. We choose however to derive the model on our own. By doing so wecould verify the existing models with ours and gain insight to the problem. Herewe show the important steps of the model derivation.
The robot is modelled as shown in gure 1 and gure 2. The proposed model de-scribes the 3-DOF dynamics of the robot and is derived using Euler/Lagrange'smethod. The Lagrangian expresses the dierence between the kinetic energyand potential energy of the robot, L = T − U . In order to express the kineticenergy for the robot three generalized velocities is needed, the linear velocity v,the angular pitch velocity ψ and the jaw angular velocity φ. The generalizedvelocities can be chosen in many ways, here we have decided for these velocitiesbecause they correspond to the variables we wish to control.
The Euler/Lagrange equations of motion for the robot are given by
d
dt
(∂L
∂qi
)− ∂L
∂qi= Qi i = 1, 2, ..., n (1)
Where q = (x, φ, ψ) are the generalized coordinates linear position, jaw angle,and pitch angle and q are the corresponding generalized velocities. Qi denotesthe generalized force for each coordinate.
3
Figure 1: Side view of the robot.
Figure 2: Robot viewed from above
4
The physical parameters of the robot are the following.
g [ms2 ] :Gravitational acceleration constantmb [kg] :Body massmw [kg] :Wheel massR [m] :Wheel radiusL [m] :Distance to center of gravity from wheel axleW [m] :Half body widthIp [kgm2] :Body pitch inertia momentIj [kgm2] :Body jaw inertia momentIxx [kgm2] :Body inertia moment about x axisIw [kgm2] :Wheel inertia momentJm [kgm2] :DC motor rotor inertia momentRa [Ω] :DC motor resistanceKb [V s
rad ] :DC motor back EMF constantKt
NmA :DC motor torque constant
Bm Nms :Damping ratio of the mechanical system
The kinetic energy of the body, Tb, is given as
Tb =mb
2[x2+2Lψ cosψx+L2ψ2+L2φ2 sin2 ψ]+
12
[Ixxφ2 sin2 ψ+Iyyψ2+Izzφ2 cos2 ψ]
(2)
And the kinetic energy for the wheels, Tw, is expressed as
Tw =12mwR
2(θr2
+ θl2) +
12Iw(θ2r + θl
2) = (mw +
IwR2
)(x2 +W 2φ2) (3)
The potential energy for the robot is just the potential energy for the CoG ofthe body which is
U = mbgL cosψ (4)
Then the expression for the Lagrangian is
L =[mb
2+mw +
IwR2
]x2+
[mbL
2
2+
12Ip
]ψ2+
[(mw +
IwR2
)W 2 +12Ij cos2 ψ
...+12Ixx sin2 ψ +
12mbL
2 sin2 ψ
]φ+mbL cosψxψ −mbgL cosψ (5)
Now we evaluate (1) for each of the coordinates.
5
x - coordinate[mb + 2mw + 2
IwR2
]x+mbL cos(ψ)ψ−mbL sin(ψ)ψ2 =
(Mright +Mleft)R
(6)
φ - coordinate[2(mw +
IwR2
)W 2 + Ixx sin2(ψ) + IJcos2(ψ) +mbL
2 sin2(ψ)]φ+ ...
2[mbL
2 + Ixx − IJ]
sin(ψ) cos(ψ)ψφ =W
R(Mright −Mleft) (7)
ψ - coordinate[mbL
2 + Ip]ψ +mbL cos(ψ)x−
[mbL
2 + Ixx − IJ]
sin(ψ) cos(ψ)φ2 − ...mbgL sin(ψ) = −(Mright +Mleft) (8)
whereMright andMleft are the torque exerted on right and left wheel by the DC-motor. Since we control motor voltages Vright and Vleft, and not the torquesMright and Mleft we must include the dynamics describing the relationshipbetween these variables, this relation is given by
Mright +Mleft =Kt
Ra(Vright + Vleft)− 2
JmRx− 2
R(Bm +
KtKb
Ra)x...
+ 2Jmψ + 2(Bm +KtKb
Ra)ψ (9)
Mright −Mleft =Kt
Ra(Vright − Vleft)− 2
JmW
Rφ− 2W
R(Bm +
KtKb
Ra)φ (10)
(Here we have neglected the eects of the inductance in motor circuit dynamics)
Inserting (9) into (6) and (8), and inserting (10) into (7) gives after reorderingof the terms the following relation between input voltage and motion dynamicsof the complete model.
x - coordinate[mb + 2mw + 2
(Iw + Jm)R2
]x+
2R2
(Bm+KtKb
Ra)x+(mbL cos(ψ)−2
JmR
)ψ−...
2R
(Bm +KtKb
Ra)ψ −mbL sin(ψ)ψ2 =
Kt
RRa(Vright + Vleft) (11)
6
φ - coordinate[2(mw +
Iw + JmR2
)W 2 + Ixx sin2(ψ) + IJcos2(ψ) +mbL
2 sin2(ψ)]φ+ ...
2
[[mbL
2 + Ixx − IJ]
sin(ψ) cos(ψ)ψ˙
+W 2
R2(Bm +
KtKb
Ra)
]φ...
=W
R
Kt
Ra(Vright − Vleft) (12)
ψ - coordinate
[mbL
2 + Ip + 2Jm]ψ +
[mbL cos(ψ)− 2
JmR
]x+ 2(Bm +
KtKb
Ra)ψ − ...
2R
(Bm +KtKb
Ra)x−
[mbL
2 + Ixx − IJ]
sin(ψ) cos(ψ)φ2 −mbgL sin(ψ)...
= −Kt
Ra(Vright + Vleft) (13)
Equations (11)-(13) is our proposed non-linear model of the robot. For ourpurposes however it is also necessary to have a linear representation of themodel. In our case this is done by using the small angle approximation which isa rst order Taylor expansion around the equilibrium point, ψ = 0. Althoughthis might seem like a brute approximation of the non-linear model it is actuallyquite accurate for angles up to about 15 degrees.
The linearization of equations (11)-(13) are represented by a state-space modelas
x(t) = Ax(t)+Bu(t)z(t) = Mx(t)y(t) = Cx(t)
(14)
where x(t) = (x, φ, ψ, x, φ, ψ), z(t) is the performance signal and y(t) is themeasured plant output signal. And
A =
0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 a43 a44 0 a46
0 0 0 0 a55 00 0 a64 a64 0 a66
7
B =
0 00 00 0b1 b2b3 b4b5 b6
C =
1R
WR −1 0 0 0
1R −W
R −1 0 0 00 0 0 0 0 1
For specication of the matrix elements see Appendix .
1.2 Hardware
1.2.1 Lego Mindstorms NXT
The Lego Mindstorms NXT Kit is a kit which contains a collection of Legopieces, sensors, actuators and a small computer - the NXT Brick. Thanksto the brick, computer logic could easily be incorporated into ordinary Legoconstructions. The heart of the NXT Brick is an ARM 32-bit microprocessorwhich can be programmed by the user.
To to able to compile and upload a program to the NXT Brick some sort ofdevelopment tool is needed. There are several dierent ones to choose from.We have decided to choose a development tool called RobotC. RobotC con-tains a complete development environment together with its own programminglanguage. The language is very similar to C, except for some minor dierences.The reason why we chose RobotC is that it is very easy to work with and it hasa lot of built-in features, like e.g. data logging, gamepad support, debugging,wireless Bluetooth communication, sensor drivers and so on.
1.2.2 Sensors and actuators
To be able to interact with the outside environment the NXT Brick can beequipped with dierent sensors and actuators. Some of them are included inthe Lego Mindstorms NXT kit others have to be ordered separately.
Gyro sensor A gyro sensor can be used to measure the angular velocity thatthe gyro sensor senses. In this project a Hitechnic NXT Gyro Sensor [4] isused for measuring the pitch angular velocity of the robot. An issue that hasto be taken in consideration, when reading the gyro sensor, is that the angularvelocity readings are biased. One can observe this phenomenon by reading the
8
raw sensor value under dierent ambient conditions and notice that the readingsdier even though the sensor is not moving. In this project the issue is solvedby subtracting the average over the gyro sensor values for about one secondduring start-up from all the upcoming readings. The bias is then assumed tobe constant during the time the robot balances.
Light sensor The light sensor that are part of the Lego Mindstorms NXTkit [5] is used to get a number that is proportional to the sensed light intensity.This sensor is used to make it possible for the robot to follow a dark line. Thesensors can be put in either active or passive mode. Active means thatlight will be emitted from the sensor. This will make the sensor readings lesssensitive to varying ambient light conditions.
Motors and encoders Two dc motors [6] are used to produce the torquesneeded to balance the robot. They are both included in the Lego MindstormsNXT kit. The analog dc voltages necessary to drive the motors are generatedby the NXT Brick. For more information about the motors see [7] and [8].
Each motor also includes an encoder. The encoder's job is to measure thenumber of steps (in units of 1°) that the motor axle has turned. This informationcould then be used as an input to the controllers used for balancing the robot.
9
2 Control
This section presents the controllers we have designed in this project. We willgive some brief theory for the dierent controller types that we have imple-mented (PID and LQG). For more detailed descriptions we recommend [9, 10].A complete description of our regulators, simulation results are also presented.
Since we have only three measured signals but 6 states in the statespace modelwe must use an observer. We have choosen to use a Kalman lter for thispurpouse, which is described below.
The state space model we are using in the design is the discrete version of (14)
x(n+ 1) = Ax(n) + Bu(n) + Hw(n)z(n) = Mx(n) (15)
y(n) = Cx(n) + v(n)
Where A, B and C are the discretized Ac, Bc and Cc matrices. w(t) are systemdisturbances and v(t) are measurement disturbances.
2.1 Kalman ltering
2.1.1 Short about Kalman ltering
If not all states for a system could be directly measured a Kalman lter maybe used to get estimates for these states. If the system disturbances w(t) andthe measurement disturbances v(t) are white Gaussian noise and the system islinear, the Kalman lter is the optimal estimator in the sense that it minimizesthe estimation error variance. If covariance/cross covariance matrices for w(t)and v(t) are known and given by
R1 = E[w(t)w(t)T ], R2 = E[v(t)v(t)T ] R12 = E[w(t)v(t)T ]
the the associated Ricati equation could be solved to obtain a Kalman gain K.On state space form the Kalman system could be written as
x(n+ 1) = Ax(n) + Bu(n) + K[Cx(n)− y(n)] (16)
Where x is estimates of the states x.
In most casesR1, R2 and R12 are not exactly known. The Kalman lter couldstill be constructed in a sensible manner by regarding R1, R2 andR12 as de-sign variables. If one for instance believe that there are large model errorscompared to measurement errors, R1 and R2 should be chosen accordingly, i.e.the elements of R1 should be large compared to the elements of R2.
10
If R2 is chosen small the Kalman lter will trust the measurements more andbecome faster to respond on changes in the measured signals but this will alsomake the estimations more sensitive to measurement noise. There is thus atrade o of making a fast lter and a noise insensitive lter. By studying theKalman estimates from test runs on the real system one can adjust R1 and R2
so that this trade o becomes appropriate.
2.1.2 Our Kalman lter
In our system we have three measured signals, left shaft angle, right shaft angleand pitch angular velocity. Note that of these measurements it is only the pitchangular velocity that is an actual state in our model. In the design we have forsimplicity assumed that R12 = 0. Since we believe that we have large modelerrors compared to measurement errors we started with a large value of R1
compared to R2. Test runs on the real system showed that we must have afast responding lter in order to be able to track the rapidly changing values ofψand ψ.
Discussion on the form of R1 and R2 The process noise we use in the designof the Kalman lter is modeled as an input signal disturbance, H = B, enteringthe system via the input signal. For our model this will have a direct eect onthe derivatives of states representing derivatives of physical quantities (velocity,jaw angular velocity and pitch angular velocity). This way model disturbancesacts on the system like forces aecting the states indirectly through the model.This is a more physically realistic modeling then adding disturbances directlyto all states.
It must be mentioned that we tried the approach of adding disturbances to allstates directly at the beginning of the Kalman lter design (i.e. H = I). Thisgave good results in simulation but on the real system the estimates directlydiverged. We have not fully realized why this happened. Using this approach, ofadding disturbance to all states, the obtained regulator system became unstable(A −BL −KC had poles outside the unit circle). Even though it should bepossible to use an unstable regulator system, something went wrong on the realsystem. When modeling disturbances as input signal disturbances we managedto locate all poles for the combined Kalman and feedback gain system withinthe unit circle, which makes it impossible for the estimates to diverge and goodestimates were obtained.
2.2 PID regulator
2.2.1 Short about the regulator
PID regulators consist of three parts, one proportional part, one integratingpart and one dierentiating part. The integrating part will regulate out static
11
errors but also make the system more oscillating, the dierentiating part willdamp the system. PID controllers are not model based and could often givegood regulation of quite complex systems if tuned properly. In the frequencydomain the regulator could be expressed as
U(s) = Kp
[E(s) +
1Tis
E(s) + TdsE(s)]
(17)
where U(s) is the control signal E(s) is the control error and the three dierentterms in the parenthesis is the proportional, integrating and dierentiating part.The dierentiating part as it stands is very sensitive to noise and is often replacedby a non ideal dierentiation, i.e. Tds is replaced by
Tds
αTds+ 1
here α is the parameter which decides how close to ideal dierentiation onewould like to get.
Also for implementation purposes we would like to represent the PID controller,in (17), on state space form which is given by,
u(t) =[Kp −Kp
] [ xi(t)xd(t)
]+Kp(1 + 1
α )e(t)[xi(t)xd(t)
]=[
0 00 − 1
αTd
] [xi(t)xd(t)
]+[ 1
Ti1
α2Td
]e(t)
(18)
2.2.2 Our regulator
In our linearized model the pitch angle and jaw angle are completely separated,i.e. there are no cross couplings between these states. To control the robotwe then use two separate PID control systems, (18), one to regulate pitch andanother one to regulate jaw. The output signals of these regulators are thenadded to form the nal control signal. Also since the state are not measureddirectly we use a kalman lter for state estimation. A schematic picture of theobserver based PID control system is shown in Figure 3.
12
Figure 3: Figure shows a schematic picture of the complete PID controller andits internal connections. PID ψ and PID φ is the controllers which controlspitch and jaw respectively and r is the reference signal for ψ and φ.
2.2.3 Tuning of the regulator
There are several ways to tune these regulators, a popular tuning scheme isZiegler-Nichols but unfortunately it only applies to stable systems. There arealso more systematic ways to tune PID regulators, such as lead-lag compensa-tion. Since we considered the PID only as a side project we didn't want to puteort into this controller and the tuning were done by feeling
2.2.4 Simulation results
In order to evaluate the regulator we have performed simulations where we con-trol the discrete linear robot model using the control method described above.The simulation experiments test the performance in terms of balancing, turningand the linear running of the robot, which are considered to be fundamentalmotions of a two wheeled inverted pendulum robot. In the following sectionthese results are presented for the PID regulator. The plots show the statesin consideration, the estimates of these states and the reference values. Thecontrol signal is also shown as the last plot in each graph. The control signal inthe real system have a maximum amplitude of 8 V, as long as it is kept underthis value it is not saturated.
13
Balancing Control The balancing performance is evaluated by giving therobot a pitch velocity of 40 deg/s, while standing in an upright position. Figure4 shows the results. Here we can see that the robot returns to its uprightposition after about 2.5 seconds.
Jaw control The turning performance is evaluated for the regulator by givingthe robot a step reference for the jaw angle, while standing still at an uprightposition. Figure 5 shows the simulation results. We can see that the robotovershoots about 5 degrees and reaches the reference after about 2 seconds. Itshould be noted that the pitch angle is at zero degrees during the response.This is because in the linear model there are no cross couplings between the jawangle and the pitch angle. We therefore expect the robot to perform slightlydierent than this simulation in reality.
Pseudo linear running control The controller has no explicit linear run-ning control. This linear running motion can however be performed by rampingup the pitch reference and bringing it back down to zero. When this is donethe controller accelerates the robot in the same direction as the pitch referencein order to balance the robot. When the reference is brought back to zero thecontroller has to keep moving the robot in the same direction with a constantspeed in order to overcome the inertia of the robot. The performance is evalu-ated by, as described, giving the robot a ramp up and ramp down reference forthe pitch angle, while standing still at an upright position. Figure 6 shows thesimulation results. Here we can see that the robot reaches a velocity of 0.2 m/sand is back at it's upright position after about 2 seconds.
14
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04
0.045
Time [s]
x [m]
stateestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
Time [s]
pitch
[de
g]
staterefereceestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−20
−10
0
10
20
30
40
Time [s]
pitch
ve
l[d
eg
/s]
stateestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.2
0
0.2
0.4
0.6
0.8
1
1.2
Time [s]
Co
ntr
ol sig
na
lIn
pu
t [V
]
right inputleft input
Figure 4: The x position, pitch, pitch velocity and control signal is shown whenthe robot is given a pitch velocity of 40 deg/sec at time 0.
0 0.5 1 1.5 2 2.5−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Time [s]
x [m]
0 0.5 1 1.5 2 2.50
5
10
15
20
25
30
Time [s]
jaw
[de
g]
0 0.5 1 1.5 2 2.5−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Time [s]
pitch
[de
g]
0 0.5 1 1.5 2 2.5−15
−10
−5
0
5
10
15
Time [s]
Co
ntr
ol sig
na
lIn
pu
t [V
]
stateestimate of state
staterefereceestimate of state
staterefereceestimate of state
right inputleft input
Figure 5: x, jaw, pitch and control signal is shown when a step of 25 deg in jawreference is set at time 2.5 s.
15
0 0.5 1 1.5 2 2.5 3 3.5−0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
Time [s]
x [m]
stateestimate of state
0 0.5 1 1.5 2 2.5 3 3.5−1
0
1
2
3
4
5
Time [s]
pitch
[de
g]
staterefereceestimate of state
0 0.5 1 1.5 2 2.5 3 3.5−0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
Time [s]
ve
l[m
/s]
stateestimate of state
0 0.5 1 1.5 2 2.5 3 3.5−0.5
0
0.5
1
1.5
2
2.5
3
Time [s]
Co
ntr
ol sig
na
lIn
pu
t [V
]
right inputleft input
Figure 6: The x position, pitch, velocity and control signal is shown when aramp up and ramp down reference is applied at time 0.5 - 1.25 s.
2.3 LQG Regulator with integral action
2.3.1 Short about LQG regulators
LQG regulators could be designed for linear models on state space form. Ifthe system is linear and the disturbances are white Gaussian noise the LQGregulator will be the optimal regulator in the sense that it is the regulator thatminimizes the criteria
J =∑n
zT(n)Q1z(n) + uT(n)Q2u(n) (19)
Where z is the performance signal, u is the input signal and Q1and Q2 aresymmetric weighting matrices, which in most cases are diagonal. The solutionto this problem is found by solving the associated Ricatti equation. From thisa state feedback matrix L is obtained which will serve as regulator.
If not all states could be measured a Kalman lter could be used as an observer.It could be shown that the design of the Kalman lter and the gain matrix Lare independent of each other. This greatly simplies the design procedure.
LQG regulators will often provide good regulation even if the situation is notideal. The model could be slightly nonlinear and the disturbances could devi-ate somewhat from white Gaussian noise. It is also quite easy to tune these
16
regulators by adjusting the weighting matrices Q1 and Q2 appropriately in aniterative process. This is two of the reasons why we have chosen to use thisregulator.
2.3.2 Design of LQG regulator
Making the regulator a servo controller The robot should be able tofollow a reference signal, r(n), we must therefore make a servo controller for it.If e(n) = z(n) − r(n) is the control error, which is the quantity that we wouldlike to keep small, expression (19) could be appropriately modied to create astate feedback for this situation. However it could be shown that if the referencesignal is assumed to be piecewise constant the same feedback gain matrix L asif e(n) = z(n) (i.e. r(n) = 0) would be obtained, this is the approach we willadapt. To get unit gain from the reference signal to the performance signalin stationarity, a gain matrix Lr is placed in the feed forward path. Thus thecontrol law with observer based state feedback structure is,
u(n) = −Lx(n) + Lrr(n) (20)
Introducing integral action in the regulator LQG regulators don't pro-vide inherent integral action. Since we desire integral action in the control loopin order to get rid of static errors we add a term eT
i Qiei to the criteria (19),which penalizes the integrated control error, ei(n). On state space form theintegrating system would be
ei(n+ 1) = (1− δ)ei(n) + Ts
e(n)︷ ︸︸ ︷[Mx(n)− r(n)]
(21)
Where δ is some small scalar which will take the system poles into the stabilityregion. To be able to work within the LQG framework ei(n) is introduced tothe system as a ctitious state. Thus the integrating system (21) is augmentedwith the original system (15). The augmented system then becomes
[x(n+ 1)ei(n+ 1)
]=[
A 0TsM I
] [x(n)ei(n)
]+[
B0
]u(n)+
[H 00 −TsI
] [w(n)r(n)
](22)[
z(n)ei(n)
]=[
M 00 I
] [x(n)ei(n)
](23)
y(n) =[
C 0] [ x(n)
ei(n)
]+ v(n) (24)
17
The regulator is then designed for this system. By this trick we will achieveintegral action in the regulator. Some important observations can be made here.In the system above r(n) is modeled as a disturbance, this is valid since fromthe controllers point of view r(n) can be regarded as just that, a disturbance.Also since the extra states, in ei(n), are ctitious, and present only in thecontrol loop, the augmented system is only considered when designing the statefeedback gain matrix L. The Kalman lter design is still based on the originalsystem.
2.3.3 Schematic picture of the regulated system
Figure 7 shows a schematic picture of the complete regulator operating in closedloop.
Figure 7: A schematic picture of the closed loop system, the regulator takes asinput the reference signal r and the measured signal y and outputs the corre-sponding control signal u.
18
2.3.4 Our regulator
Our regulator is constructed as shown in Figure 7. The state-space representa-tion of the control system is given by,
u(n) = −[
Lx Li
] [ x(n)ei(n)
]+[
Lr 0] [ r(n)
y(n)
][
x(n+ 1)ei(n+ 1)
]=[
A−KC−BLx −BLi
TsM I
] [x(n)ei(n)
]+[
BLr K−TsI 0
] [r(n)y(n)
](25)
Here L =[
Lx Li].
The regulator will as performance signal use jaw angle, pitch angle and forwardvelocity. The M matrix in (15) is then given by
M =
0 1 0 0 0 00 0 1 0 0 00 0 0 1 0 0
Tuning of the regulator The regulators were tuned by adjusting the weight-ing matrices so that simulations showed good results in regulation of distur-bances and ability to follow reference signals without saturating the input sig-nal, see section 2.3.5 for simulation results. The regulator were then tested onthe real system. Since there are imperfections in the physical model the resultshere were as expected not exactly the same. By looking at the behavior of thereal system we then adjusted the weighting matrices so that we achieved goodregulation.
2.3.5 Simulation results
In order to evaluate the regulator we have performed simulations where we con-trol the discreet linear robot model using the control method described above.The simulation experiments test the performance in terms of balancing, linearrunning and turning of the robot, which are considered to be fundamental mo-tions of a two wheeled inverted pendulum robot. In the following section theseare presented for the regulator. The plots shows the states in consideration, theestimates of these states and the reference values for each state. The controlsignal is also shown as the last plot in each graph. The control signal in the realsystem have a maximum amplitude of about 8 V, as long as it is kept underthis value it is not saturated.
Balancing Control The balancing performance is evaluated for both regu-lators by giving the robot a pitch velocity of 40 degrees/s, while standing in anupright position. Figures 8 and ?? show the results for the moving and still
19
regulator. For both controllers the robot returns to its upright position afterabout 5 seconds.
Linear running control Linear running control is only implemented in themoving regulator and thus only simulated for this controller. The performanceis evaluated by giving the robot a step reference for the linear velocity, whilestanding still at an upright position. Figure 9 shows the simulation results.Here we can see that the robot reaches the velocity reference and is back at itsupright position after about 2 seconds.
Jaw control The turning performance is evaluated for both regulators bygiving the robot a step reference for the jaw angle, while standing still at anupright position. Figure 9 shows the simulation results. We can see that thefor the moving regulator the robot overshoots about 5 degrees and reachesthe reference after about 2 seconds while the still regulator has no over shootand reaches the reference with a small deviation after about 1.5 seconds. Thisis because the move controller is implemented to respond harder on the jawreference while the still regulator designed to give a softer response. It shouldbe noted here that the pitch angle is at zero degrees during the response. This isbecause in the linear model there are no cross couplings between the jaw angleand the pitch angle. We therefore expect the robot to perform slightly dierentthen this simulation.
0 1 2 3 4 5 6 7 8−0.03
−0.02
−0.01
0
0.01
0.02
0.03
0.04
Time [s]
x [m]
stateestimate of state
0 1 2 3 4 5 6 7 8−2
−1.5
−1
−0.5
0
0.5
1
Time [s]
pitc
h[d
eg]
staterefereceestimate of state
0 1 2 3 4 5 6 7 8−30
−20
−10
0
10
20
30
40
Time [s]
pitc
h ve
l[d
eg/s
]
stateestimate of state
0 1 2 3 4 5 6 7 8−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
1.2
Time [s]
Con
trol
sig
nal
Inpu
t [V
]
right inputleft input
Figure 8: The x position, pitch, pitch angular velocity and control signal isshown when the robot is given a pitch velocity of 40 deg/sec at time 0.
20
0 1 2 3 4 5−0.2
0
0.2
0.4
0.6
0.8
1
1.2
Time [s]
x [m]
stateestimate of state
0 1 2 3 4 5−2
0
2
4
6
8
10
Time [s]
pitc
h[d
eg]
staterefereceestimate of state
0 1 2 3 4 5−0.4
−0.3
−0.2
−0.1
0
0.1
0.2
0.3
Time [s]
vel
[m/s
]
staterefereceestimate of state
0 1 2 3 4 5−8
−6
−4
−2
0
2
4
Time [s]
Con
trol
sig
nal
Inpu
t [V
]
right inputleft input
Figure 9: The x position, pitch, velocity and control signal is shown when a stepof 0.2 m/s in velocity reference is applied at time 0.5 s.
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Time [s]
x [m]
stateestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
5
10
15
20
25
30
Time [s]
jaw
[deg
]
staterefereceestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Time [s]
pitc
h[d
eg]
staterefereceestimate of state
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−5
−4
−3
−2
−1
0
1
2
3
4
5
Time [s]
Con
trol
sig
nal
Inpu
t [V
]
right inputleft input
Figure 10: x, jaw, pitch and control signal is shown when a step of 25 deg injaw reference is set at time 2.5 s.
21
Simulation results LQG Regulator
3 Line tracker
3.1 Introduction
It is desired that the robot is able to follow a dark line. To sense the line therobot uses a light sensor. Based on the reected light intensity the robot isable to distinguish between a dark area (on the track) and a light area (othe track). This is done by comparing the light sensor value with a thresholdvalue, with some hysterisis. If the value is less than the threshold the robot isassumed to be on the track, otherwise it is assumed to be o the track. Thatinformation is then used by the line tracking algorithm to stay on the track.
There is no real restriction on how the track could be shaped, as long as thetrack is continous. So the robot software should not make use of any a prioriinformation about the track shape in its line-tracking algorithm.
An example on how a track could look like is shown in Figure 11. This trackalso available as a print-out. Unfortunately the line is printed using a ratherlight green color. This will make it harder to distinguize between on track ando track
Figure 11: The Eight ShapedTrack
3.2 The algorithm
The idea behind the line-tracking algorithm is to stop and sweep the area infront of the robot when the robot is not on the track. The sweeping is performedby rst turning the jaw a maximum of eg. 10° in one direction. When it is donethe robot will return back to the initial jaw angle, i.e. the angle where it lost thetrack. The robot will then try to sweep the area in the opposite direction. Thistime the maximum sweep angle has been extended by eg. 5°. The procedure isthen repeated until the line is located. So the sweeping area will keep growingat every direction change. As soon has the line has been located the robot stopsthe sweeping and starts to move forward. The algorithm remembers in whichdirection the track was found last time. So the next time the robot comes othe track the algorithm starts sweeping in that direction.
22
A detailed ow-chart on how the line-tracker algorithm works is depicted inFigure 12. One loop is performed every sample period. For a more illustrativeexample see Figure 13.
Figure 12: Line-tracker algorithm
23
Figure 13: Example run of the line-tracking algorithm
24
4 Software
4.1 Lego software
Once a regulator has been developed on the PC it is nally time to test theregulator on the real robot. The task for the Lego software is mainly to try tobalance the robot using that regulator. Apart from that the software has somemore features built-in.
4.1.1 Functionality
A feature which comes in handy when evaluating the performance of a controlleris the data logger. With the logger it is possible to log the variables holdinginformation about the input to the controller (references and sensor values), theoutput from the controller (the two motor voltages) and the internal states ofthe controller to a PC (via USB or Bluetooth). To save on the available datalog memory it is possible to choose exactly which regulator inputs, outputs andstates that should be logged. This feature is almost necessary since the data login the robot has only room for about 1500 data points. The logs can then betransferred to a PC for evaluation in a Matlab script. See Appendix for moredetailed information.
The robot can take input from a gamepad connected to a PC. The buttonscontrol the robot by applying reference signals to the controller. The referencesare applied gradually by ramping up them to some nal value in smaller steps.
One of the gamepad buttons is used to activate/deactivate the line tracker. Therobot is then able to follow a dark line.
The robot can be stopped and then restarted by the press of another gamepadbutton.
4.1.2 Implementation
The program is implemented using only one task. The reason for this is to avoidthe complexity that comes with multitasking systems (overhead, synchroniza-tion, deadlines etc.).
Once the program has initialized everything it needs it will enter the big controlloop. It is in here where everything happens. See Figure 14 for a full ow-chartof the program.
Since the controller, which is supposed to balance the robot, is designed fora certain sampling frequency it is important that all timings get correct. Forexample the control signal should be applied at the same time instant as thesensors have been read and the new control signal has been calculated. That isthe reason why the more computational heavy parts are postponed until after
25
the control signal has been applied. It is also vital that the control loop does notexceed the sample period. For that reason a control mechanism is implementedwhich will simply stop the robot if the sample period is overdued.
Unfortunately, for the gamepad to work the sample period sometimes might beexceeded by 1-3 ms. To get as little inuence on the balancing as possible thegamepad is only read once every 200 ms. With such low read frequency thebalancing should not be aected noticeably.
The controllers are represented in a linear state-space form. Calculating thecontrol signal and iterating the internal states of the controller therefore in-volve performing matrix-vector multiplications. Those operations become quitedemanding for the Lego system as the state-space matrices become large. An al-ternative method to writing general matrix-vector multiplication routines (typ-ically involving for-loops) is to generate a set of sums of scalar multiplicationsin RobotC code. The latter method is used in this program. All matrix-vectormultiplications that are to be performed by the robot are generated by a Matlabscript. Given a controller the script generates the RobotC code les necessaryfor the Lego software to operate the controller. This method also gives the scriptthe opportunity to simply skip generating multiplications for matrix elementswhich are zero.
26
Figure 14: Robot software owchart
27
5 Appendix
A =
0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 a43 a44 0 a46
0 0 0 0 a55 00 0 a64 a64 0 a66
den =(mb L
2 + Ip + 2Jm) (
mb + 2(mw +Iw + JmR2
))−(mbL−
2 JmR
)2
a43 = −(mbL− 2 Jm
R
)mb Lg
den
a44 = −2R2
(Bm + Kb Kt
Ra
)((R+ L)mbL+ Ip)
den
a46 =2R
(Bm + Kb Kt
Ra
)((R+ L)mbL+ Ip)
den
a55 = −2W 2
(Bm + Kb Kt
Ra
)R2 (Ij + 2W 2mw) + 2W 2 (Iw + Jm)
a63 =
(mb + 2(mw + Iw+Jm
R2 ))mb Lg
den
a64 =2R2
(Bm + Kb Kt
Ra
) ((R+ L)mb + 2(mw + Iw
R2 )R)
den
a66 = −2R
(Bm + Kb Kt
Ra
) ((R+ L)mb + 2(mw + Iw
R2 )R)
den
B =
0 00 00 0b1 b2b3 b4b5 b6
28
b1 =Kt
RRa((R+ L)mbL+ Ip)
den, b2 = b1
b3 =Kt
RRaW
(IJ + 2(mw + Iw+Jm
R2 )W 2), b4 = −b3
b5 = −Kt
RRa
((R+ L)mb + 2(mw + Iw
R2 )R)
den, b6 = b5
C =
1R
WR −1 0 0 0
1R −W
R −1 0 0 00 0 0 0 0 1
5.1 Overview of the programming design process
A schematic picture of how we construct and test regulators is shown in Figure15.
Figure 15: A schematic picture showing the process of creating a regulator andtesting it on the real system.
29
A more detailed explanation of each block in Figure 15 is given below.
5.1.1 In Matlab
Initialization
Constants and state space model are loaded from constants.m and statespace_model.mrespectively.
Create regulator
The regulator is constructed by specifying the needed regulator parameters andthen calling lq_servo.m/pid_reg.m with these parameters as input variables.These functions will give as output a complete regulator system on state spaceform, including the Kalman system.
Simulate
Simulations of the linearized system is done by running sys_sim.m which willreturn the simulation results.
Analyse simulation results
The obtained results from execution of sys_sim.m are presented by using plot-ter.m which will display simulation plots. draw_model.m will show a simpleanimation of the simulation.
Export regulator to RobotC
If the simulation results look satisfactory the regulator is exported to RobotCby calling sys2c.m.
5.1.2 In RobotC
Run the software on the Lego device
To build and upload the RobotC program to the robot start RobotC for Mind-storms and press F5. Make sure that the robot is not moving. Then press F5once more to start the code execution. The robot will then make a short beep.When the gyro bias has been calculated the robot will play a short melody. Thismeans that you shall place the robot in the upright balancing position. Afterthe third and last beep the robot is ready to balance!
30
Control the robot from a gamepad
Once the robot is balancing the robot can be controlled by a gamepad whichis connected to a PC. Make sure that the RobotC gamepad window is openedfrom the Robot -> Debug -> Joystick menu. The following functions can thenbe called by pressing the corresponding gamepad button.
Command Button
Inc. velocity 4Dec. velocity 1Turn left 3Turn right 2Active/deactive line-tracker 6Stop/start 7Calibrate line-tracker 8Horn 5
Activate data logging
To activate the data logger choose the le log_settings.c from the Includemenu. Then set the variables that you wish to log to the digit 1 and the thosethat you do not want to log to the digit 0. Then active the logging by settingLOG_DATA to 1.
Analyze logged data
The logged data rst has to be transfered to the PC. This is done from theNXT Brick -> File management menu. The data log is named DATAxxxx.Choose Upload spreadsheet to download the le to the PC. Change to thedirectory containing all the RobotC source les. Then open the le show_datalog.m.
Specify the lename of the saved data log. Then run that cell by pressing[Ctrl]+[Enter]. This will load the lename and the regulator to the workspace.
In the next cell change log_period to reect how often the data was saved,typically every sample period (lego_reg.Ts). The NUM_DEC tells how manydecimals were used when the data was logged. See DECIMAL_FAC in theRobotC source le log_settings.c for more information. Then run the cell([Ctrl]+[Enter]) to load the saved data to workspace.
Finally choose which variables that you wish to plot and run that cell ([Ctrl]+[Enter]).
31
References
[1] Yamamoto Y, (2008) NXTway-GS Model-Based Design - Control of self-balancing two-wheeled robot built with LEGO Mindstorms NXT
[2] Jeong S, Takahshi T (2007) Wheeled inverted pendulum type assistantrobot: Design concept and mobile control.
[3] Grasser F, D'arigo A, Colombi S, Rufer A (2001) JOE: A mobile, invertedpedulum
[4] http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NGY1044
[5] http://mindstorms.lego.com/Products/Sensors/Light%20Sensor.aspx
[6] http://mindstorms.lego.com/Products/Accessories/Interactive%20Servo%20Motor.aspx
[7] http://www.philohome.com/nxtmotor/nxtmotor.htm
[8] http://web.mac.com/ryo_watanabe/iWeb/Ryo%27s%20Holiday/NXT%20Motor.html
[9] Glad T, Ljung L (2006) Reglerteknik grundläggande teori
[10] Glad T, Ljung L (2003) Reglerteori ervariabla och olinjära metoder
32