Embedded Control Systems
LegoWay
Simon Berthilsson
Anders Danmark
Ulf Hammarqvist
Hanna Nygren
Vasilij Savin
2009-10-18
Contents
1 Background
1.1 Goal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 State of the art . . . . . . . . . . . . . . . . . . . . . . . . . .
3
3
3
2 Problem solution
2.1 Approach . . . . . . . . . . .
2.2 Model design . . . . . . . . .
2.2.1 Controller design . . .
2.2.2 State space equations
2.3 Hardware . . . . . . . . . . .
2.3.1 Lego Mindstorms Nxt
2.3.2 Sensors and Actuators
2.4 Software . . . . . . . . . . . .
2.4.1 MatLab . . . . . . . .
2.4.2 SimuLink . . . . . . .
2.4.3 RobotC . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
3
3
3
3
5
6
6
6
6
6
7
7
.
.
.
.
.
.
.
.
.
.
.
.
7
7
10
12
12
13
13
15
16
17
17
17
18
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3 Implementation
3.1 Mathematical model . . . . . . . . . .
3.1.1 NXTway-GS model . . . . . .
3.2 Deriving the state space equations . .
3.2.1 Kalman filter . . . . . . . . . .
3.2.2 LQG controller . . . . . . . . .
3.3 Simulation . . . . . . . . . . . . . . . .
3.4 Constructing the robot . . . . . . . . .
3.5 Algorithm for following a line . . . . .
3.5.1 Sensor reading . . . . . . . . .
3.5.2 Movement control . . . . . . .
3.6 Experimental results and observations
3.6.1 PWM issues . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Software Structure
19
5 Conclusions
20
6 Appendix
22
2
1
Background
1.1
Goal
The goal of the LegoWay project is to find a cheap and reliable solution
to balance a two wheeled inverted pendulum. The pendulum itself is to be
constructed and controlled by LEGO Mindstorm NXT. The robot should
also be able to follow a path (specified by a line drawn on the floor) while
balancing on two wheels. This should all be done with one gyro sensor
and one light sensor. The robot is going to be programmed using RobotC
development platform.
1.2
State of the art
Several solutions to the wheel inverted pendulum (TWIP) problem have
been published on the Internet, probably the most relevant one is the NXTwayGS where GS is short for Gyro Sensor. The NXTway-GS is based upon a
LQG controller and a gyro sensor. The performance of the NXTway-GS is
actually quite impressive. Of all inverted-lego-pendulums we have studied,
this is probably the one which performs the best and that is in turn probably
because it utilizes an LQG controller. [1]
2
Problem solution
2.1
Approach
The task of building an inverted pendulum, capable of following line, has
been split into several smaller subtasks:
• a line following module
• a controller for balancing
• mathematical model
• controller implementation
• a main control program.
2.2
2.2.1
Model design
Controller design
There are several different types of controllers. One of the most basic is
the PID-controller. The derivation of a PID controller is fairly straightforward and while the control can be rather good, there are limitations. The
inverted pendulum system is an unstable system and requires an accurate
3
and fast control. It is definitely possible to control such a system with a
PID regulator but this type of control will be limited in performance.
An LQG (linear-quadratic-Gaussian) controller has the capability to utilize
more known information about the system and create a control which is
better adapted to each situation. The main drawback of using a LQG controller over a PID controller is a more complicated derivation and that it
requires more computational power to run. There are of course several more
regulators as well, the trade-off that it always boils down to is complexity
versus performance under certain conditions.
For instance, Model Predictive Controllers are closely related and could most
likely have been used as well. MPC predicts the systems states a number of
time steps ahead and regulates so that the prediction of the states is within
desired bounds.
The conditions that are to be dealt with in this case are that the system is
unstable and non-linear but well known and well estimated by a linearization around the working point. There are some regulators that can handle
unknown systems by means of on-line system identification and other regulators which performs well with non-linear systems. But since the system
in this case is fairly well known it was concluded there was no need to use
adaptive controllers. The LQG controller should suffice for project needs
since the system is rather well known and can be approximated as a linear
system for angles within 10 degrees. The LQG controllers are also relatively
easy to implement and design with Matlab. Matlab has a lot of built in
functions(and help documents) made to aid design of LQG regulators.
When the model has been derived and the states are chosen, the final step
in the design of the LQG controller is to assign proper weights of the different states against each other. This stage of the design process is highly
iterative and some experimentation may be needed in order to produce a
well behaving controller.
The construction of an LQG controller requires a mathematical model of
the controlled system. In the ideal case you can measure every interesting
property of a system and calculate the control signals with a feedback gain.
Since we can only measure angular speed of the body and angular position
of the wheels we need some kind of estimate of the remaining states. A
commonly used way of estimating such states in control theory is using an
observer. This project uses a Kalman filter, since it is an optimal linear
observer. Construction of a Kalman filter has the following pre-requisites:
• a state space model of the system
4
• an estimate of the noise in the sensors
• an estimate of the noise in the actuators
These estimates can be considered design variables and it is not necessary
to get perfect descriptions of the noises during the first attempts since satisfying results can be achieved anyway.
In order to be able to simulate the system reliably a mathematical description of the non-linear system is needed. Obviously with a higher degree of
accuracy a better correlation between simulations and reality is obtained.
However, in order to get a result, a linearised model can be used instead
of a non-linear model. In the end, it will come down to a tweaking process
on the actual physical implementation anyway so as long as the linearised
model is accurate in the region of interest it’s an acceptable approach.
The controller is usually derived as dynamic system, while microprocessors
operate with discrete values. This difference should be accounted for. The
simplest way of doing this is to simply ignore the problem and assume that
the sampling rate is sufficiently fast in relation to the speed with which the
system reacts. When this assumption does not hold - the system is faster
than sampling rate - the regulator has to be sampled. In MATLAB This
can be achieved with the command ’c2d’. It is important to remember that
this can affect the stability of the system and that some additional tuning
may be needed in order to get the discrete system stable.
2.2.2
State space equations
State space form is a common way of describing a system. It is based on
creating a vector space of the states of the system in such a way that every
possible state the system can assume may be described by a vector in this
space. Because of this it is important to choose state variables in such a way
that the vector space becomes linearly independent. There are a few other
important parameters that should be considered in choosing state variables:
• Observability; a measure of how well the states correspond with reality.
• Controllability; that given a state , any other state can be reached
during some amount of time.
The state space form of model can be described by a set of matrix equation
ẋ = Ax + Bu
y = Cx + Du
(1)
where x is the vector of state variables and u - the vector of control signals
5
2.3
2.3.1
Hardware
Lego Mindstorms Nxt
Lego Mindstorms Nxt is a programmable robotics kit released by Lego in
July 20061. The main component is Robot Computer Core, which is also
called the Brick. It has four sensor ports for input and three motor ports for
output. All ports peripherals are connected using RJ12 cables. The main
CPU is a 32-bit ARM processor which runs at 48 MHz. Users can upload
programs to the robot using USB cable or Bluetooth. The brick firmware
is released as open source. Due to the open source license there exists a
number of different unofficial languages, RobotC, Lejos, NXC and NBC to
name a few. This has also led to the development of third-party devices
such as the HiTechnic Gyro Sensor.
2.3.2
Sensors and Actuators
In this project we will utilize three different sensors. Rotary encoders in the
motors, the HiTecnic gyro sensor and the stock Lego light sensor. In this
project two Lego DC motors are used as actuators.
Sensor/Actuator
DC Motor
Rotary Encoder
Light Sensor
Gyro: HT
Output/Input
PWM
Angle
Integer Value 0-100
Angular Speed
Unit
%
degree
n/a
deg/sec
Sampling Rate (Hz)
500
1000
300
300
Table 1: Sensor and actuator data
When designing real-time software it is especially important to make your
program predictable. An usual solution is to a have a number of independent
tasks that communicate with each other via shared memory or semaphores.
One thing to consider is that all tasks should have time to execute. There
are a number of different algorithms to control schedulability.
2.4
2.4.1
Software
MatLab
Matlab is a popular numerical computing environment. It is used both in the
industry as well as the academic world. Matlab has numerous extensions,
called toolboxes, among other cone called Control Systems toolbox. We will
use Matlab and Control Systems Toolbox as our primary environment for
creating our controller.
6
2.4.2
SimuLink
SimuLink is a very flexible and intuitive way of designing systems since it
uses a graphical interface. It’s bundled together with Matlab and the programs are closely linked. Matlab functions can be ran from within SimuLink,
and it can import/export to Matlab’s workspace. This makes it ideal for
simulations and visualising the control structure.
2.4.3
RobotC
ROBOTC is a powerful C-based programming language with a Windows
environment for writing and debugging programs, and the only programming
language at this level that offers a comprehensive, real-time debugger. We
will use RobotC for programming and debugging our robot.
3
Implementation
A mathematical model of a non-flexible body is preferred since flexible dynamics is much more complex than non-flexible. Secondly, the centre of
mass preferably should be centred over the wheel axis. This will help the
balancing and facilitate the derivation of the moments of inertia.
3.1
Mathematical model
A successful regulation requires a mathematical model that is accurate
enough for the specific needs of the control system. Initially, an attempt
was made to model the system with Lagrangian dynamics. The generalized
coordinates were chosen according to: φl , φr and θ where φl represents the
angle of the left wheel, φr the angle of the right wheel and θ the tilt angle
of the robot itself. This is shown in figure 1.
7
Figure 1: Generalized coordinates for our mathematical model
The used constans are shown in Table 2.
Iα
Iθ
m
τR,L
hm
g
r
b
λ
The moment of inertia around the α-axis
The moment of inertia around the θ-axis.
The mass of the robot,
the respective torques of motor R and L
The height from the wheel axis to the centre of mass
The gravitational acceleration constant
The wheel radius
The distance between the wheels.
A constant, describing how the torques of the motors
affect the body of the robot.
Table 2: Constants used in the derivation
The state equations using these coordinates will look like this (full derivation: see appendix):
φ¨L =
φ¨R = φ˙L (
Tb
TA b
TA
− φ˙R b + τR − τL
)
(φ˙L
2
T
T
− TA )
B
B
r2 (TB2
(2)
−TA
b
b
τR
τL
−
)+ φ˙R 2 2
− (T 2 −TA2 )+
(3)
TB r2
r2 (TB2 − TA2 ) TB r2
r (TB − TA2 r2 B
θ̈ =
−mghm
cos(θ) − λτL − λτR
Iθ
8
(4)
TA = π 2 m +
Iα
b2
(5)
Iα
(6)
b2
This model was never implemented or tested. It turned out that deriving
such a model and putting it on state space form takes a lot of time and work.
Especially the derivation of the moment of inertia around α-axis proved to
be problematic since it changes with the tilt angle θ. The proposed solution
to this problem was to use calculate a table of Iα for different values θ, and
then use a look-up table with the closest corresponding value on θ but this
was never implemented.
TB = π 2 m −
At this point in deriving the model we realized it would take us too long to
finish. So instead of deriving our own model we choose to carry on with an
existing model, the one found in the NXTway-GS project, which is known
to work.
9
3.1.1
NXTway-GS model
The NXTway-GS model use three generalized coordinates (θ,φ,ψ) which describes average wheel angle, yaw angle and tilt angle respectively. See Figure
2. The used constants can be found in Table 3 .
Figure 2: NXTway-GS Model with generalized coordinates
10
Constant value
g=9.82
m=0.018
M=0.584
W=0.16
D=0.05
H=0.16
L=H*2/3
Unit
[m/s*s]
[kg]
[kg]
[m]
[m]
[m]
[m]
R=0.031
JW =m*R*R/2
Jψ =M*L*L/3
Jφ =M*(W*W+D*D)/12
Jm =1e-5
Rm =6.69
Kb =0.468
Jt =0.317
n=1
fm=0.0022
[m]
[kg*m*m]
[kg*m*m]
[kg*m*m]
[kg*m*m]
[ohm]
[V sec/rad]
[Nm/A]
fw=0
Description
Gravity acceleration
Wheel mass
Body mass
Body width
Body depth
Body
Distance to the centre of
mass from wheel axle
Wheel radius
Wheel moment of inertia
Body pitch moment of inertia
Body yaw moment of inertia
DC motor inertia moment
DC motor resistance
DC motor back EMF constant
DC motor torque constant
Gear ratio
Friction coefficient between
body and DC motor
Friction coefficient between
body and floor
Table 3: Used constants
11
Using these constants together with Lagrangian dynamics yields the following motion equations.
θ̈(2m + M )R2 + 2J + 2n2 J) + ...
ψ̈(M LRcos(ψ) − 2n2 Jm ) − +M LRψ˙2 sin(ψ) = Fθ
θ̈(M LRcos(ψ) − 2n2 Jm ) + ψ̈(M L2 + Jψ + 2n2 Jm ) − ...
−M gLsin(ψ) − M L2 φ˙2 sin(ψ)cos(ψ) = Fψ
2
W2
2 J ) + M L2 sin2 (ψ) + ...
(J
+
n
+
J
+
φ̈ mW
m
W
φ
2
2R2
+2M L2 ψ̇ φ̇sin(ψ)cos(ψ) = Fψ
3.2
(7)
(8)
(9)
Deriving the state space equations
Using the mathematical model derived previously together with a first order
model of the DC motor and our constants, we can derive our final system
description on state space form. The result is on the form:
ẋ = A + Bx
(10)
For x = (θ, θ̇, ψ, ψ̇, φ, φ̇)T For the derivation see the appendix.
These equations need to be sampled. Choosing our sampling rate (5 ms)
was a trial and error process. It’s a bit slower than the sampling rate for the
DC motors. We tried different sampling rates from 3-10 ms and eventually
we just chose 5 ms.
3.2.1
Kalman filter
With a model in Matlab it is fairly simple to make an observer. All that is
needed now are the noise estimate matrices. We chose the following matrices.
Q1 0
Q=
 0 Q1

Q1 /100 0
0
R1 0 
R= 0
(11)
0
0 R1
Q1 = 8.726646
R1 = 0.034907
The value on Q is a design parameter that if set low makes the Kalman
filter respond faster (at the cost of noise sensivity) and if set high makes
it resistant to noise (but slow). The values of R were determined through
estimating the measurement noises and process noise. R1 corresponds to 5◦
play in the wheel gearing. Q/100 correspons to a noise of around 2◦ /s in
the gyro sensor.
12
3.2.2
LQG controller
In order to calculate the optimal feedback controller for the LQG controller
two weighting matrices are needed. One matrix for the states (Qx ) and
another for the inputs(Qu ). We chose the following matrices:
 4
10
0
0
 0 104 0

 0
0 103
Qx = 
 0
0
0

 0
0
0
0
0
0
1 0
Qu =
0 1
0
0
0
1
0
0
0
0
0
0
1
0

0
0

0

0

0
(12)
1
(13)
The values chosen are a result of an iterative process using simulations where
the initial values are simply guessed and then tweaked until desired dynamics
are achieved. The values chosen puts a lot of weight on the states that makes
the regulator follow the references on position, velocity and leaning angle.
3.3
Simulation
Once a model was chosen we started implementing a simulator in SimuLink
for testing our regulators. Simulating the linearised model was not so hard
but we never actually managed to simulate the non-linear system in spite
of days spent on troubleshooting. The problems were partly due to deriving expressions for the complex second order derivatives that could be
implemented, as well as problems with SimuLink and MatLab itself. It was
decided that it is better to spend time on the actual physical robot and
be satisfied with simulating with a linearised model. Below is a simulation(Figure 3) where the robot is instructed to go forward and turn, ie a
reference change on both position (θ) and turning (φ).
13
Figure 3: A simulation where the robot is instructed to go forward and turn,
ie a reference change on both position (θ) and turning (φ).
It can be observed that the robot responds well to reference changes for θ
and φ. Note the transients on θ̇ and ψ̇. A zoomed in version is shown on
the next page in Figure 4.
14
Figure 4: A detailed figure for (θ̇) where the robot is instructed to go forward
and turn, ie a reference change on both position (θ) and turning (φ).
Note that the robot actually goes backward before it can forward to it’s set
reference. This because the robot has to lean forward in the direction it’s
instructed to move. In order to move the body has to “fall“ with the wheels
keeping up (The transient in ψ̇ shows the same dynamics but measured in
roational velocity).
3.4
Constructing the robot
When designing our robot we had a few things to consider. A rigid physical
model is desirable, so assumptions in the mathematical model will hold.
Another consideration was the height of the robot. Initially we thought it
15
to be good idea to make it as tall as possible because after experiments
with balancing a stick in a hand, it was discovered that it was easier to
balance a longer stick. This design idea was dropped later, since the period
for the oscillation for a pendulum increased when the length increases. This
means that a higher motor speed is needed with taller bodies and the motors
could not provide the speeds required for our tall design. So the robot was
rebuilt to have a lower profile. Some attention was given to aligning the
centre of mass directly above the wheel axis in order to make for a more
naturally stable system and to accommodate certain model assumptions.
No consideration has been taken to the sensors or their placement in the
model but their masses are small enough to safely be ignored assuming they
are placed with some care.
Figure 5: Front and rear view of the robot.
1
2
3
4
3.5
NXT-brick
Motor
Infrared sensor
Gyro sensor
Algorithm for following a line
A line following algorithm is an easy problem to grasp. Firstly, some measurements are needed to determine whether the robot is on track or off track.
When you lose the line you need to find it again. In this project measurements are obtained from a stock Lego Mindstorms Light Sensor. Given that
we have only one sensor and limited computational resources, it has been
decided to use a relatively simple line following algorithm. The algorithm
involves 2 separate threads:
16
3.5.1
Sensor reading
A value from the sensor is read. Light sensors gives readings between 0
(totally dark/black) and 100 (totally white) If the value is below a threshold,
it is assumed that the robot is on track and a global variable representing
this called OnTrack is set to true. This thread also outputs some debug
information to the robot’s screen.
3.5.2
Movement control
While the global variable OnTrack is true the robot keeps going forward.
When it’s off track (OnTrack is false) corrective action is taken. This corrective action means that the robot turns on the spot to search to the side where
it last found the line. If the line is not found within some interval, the robot
turns to search the other direction and repeats this until the track is found
again. Control is being performed by inputting new reference values for the
regulator, which are later on taken into account by the balancing function
The line follower algorithm can be seen as a disturbance in the balancing
algorithm as it changes the reference values. Some tuning was needed which
concerned both the default turn angle and a few wait commands in the code.
The waits are needed because the robot might need some time in order to
reach the set reference point. Obviously there’s room for improvement in
the algorithm. An idea we never had time to investigate was some kind of
curvature fitting. Using the a number of curve edges it should be possible
to fit a curvature and make the robot follow that predicted path instead,
which would solve the zig zag movement the current algorithm produces.
3.6
Experimental results and observations
In order to speed up development and robot testing, a special Matlab script
fileio.m has been created. It runs after modell.m reads relevant data and
outputs state space equations for the observer, feedback and reference calculation in RobotC syntax. This then pasted into the RobotC IDE, compiled
and run within seconds.
We have encountered several difficulties associated with implementing the
RobotC code on the NXT brick. The most significant hardware problem
encountered was a problem that occurs when a motor is connected to Port
C on the brick. Switching to Ports A and B solved this problem. The symptoms of this configuration are that the output signals to the motors simply
are set to zero for a short period of time making the robot fall.
Another annoying problem was that sometime it was impossible to upload
new files to the robot unless they had the same name as files already in the
17
brick’s memory. This was remedied with a firmware reset.
Actual RobotC language also caused some issues. The documentation was
far from perfect and the differences from C are numerous and significant.
It caused some delays in development. Once such nuances have been mastered programming became quite straight forward. Our standard solution
to unexpected behaviour from the brick was to upload the firmware again
and every time it was not a programming error this solved the problem.
The accumulated wisdom can be summarised this way: When you can’t
program the robot over Bluetooth, upload the firmware again. If programs
does not run, upload the program again. When the robot fails to execute
the program, correctly re-upload the firmware. Always make sure to branch
the code to prevent loss of valuable data. Check your constants for example
do not mix Jφ with Jψ . Probably using CVS for this project might have
been beneficial, even if we had only a handful files to work with.
3.6.1
PWM issues
Here we had one significant problem. Our model used voltage as input but in
RobotC we could only control the motors with PWM. We had to convert out
feedback voltage to PWM. In order to convert a voltage to PWM you need to
divide with the maximum possible voltage. The maximum possible voltage
varies with battery power and a simple formula to calculate the current
maximum voltage was needed. To do this we needed a measurement of
motorspeed versus motorvoltage. From the NxtWay-Gs we got the following
figure:
18
Figure 6: A plot of the maximum speed for the motors versus voltage.
We then made a linear regression and used a first order DC-motor model.
This yielded the following formula.
Vmax = 1.0077 ∗ Vbattery − 0.633
(14)
The RobotC api provided us with a measurement of the current battery
voltage(as an integer 8400 meaning 8.4 volts). This meant we had to modify the voltage equation. This is the equation implemented in our control
program.
Vmax = 0.0010077 ∗ Vbattery − 0.633
(15)
To convert a voltage to PWM we just have to divide the voltages our controller calculated with the maximum voltage.
4
Software Structure
Main
This is the start-up task. It initializes the sensors and starts the other tasks.
MotorControl
MotorControl is basically a timer that updates the controller with sensor
data. The controller then calculates control signals for the motors using the
LQG feedback gain and reference values.
19
FollowLine
This task contain the line following algorithm. It uses different commands
for setting reference values in order to steer the robot.
GetLightReading
This task read the light sensor and decides whether the robot is on or off
the line. It communicates with FollowLine via a shared variable OnTrack.
5
Conclusions
The final result of this project is a very well balancing robot. It can follow
reference line in an reasonably quick manner. It actually balances better
than we expected it to but we have produced several controllers with different degrees of errors in them during this course. It is difficult to know
what performance that can be expected and we have believed that we have
reached the best possible regulator a few times but then, by discovering and
correcting errors, improved it dramatically. It turns out that the controller
is also sensitive to scaling issues in the state variables, meaning our regulator
behaved better when we managed to implement radians and actual PWM
values instead of using degrees and just making sure that the resulting output lies within the permitted interval. The controller is also very sensitive
to model errors. This is not at all unexpected since the regulator strives to
control the model on which it is based, things will go wrong if this differs
much from the system the controller is implemented on and thus actually
trying to control.
Work Distributions
During our work on this project each project member had different main responsibilies. Anders was responsible for the design of the regulator. Hanna
was in charge of linear simulation. Simon was responsible for model derivation. Ulf implemented the nonlinear model. Vasilij was responsible for the
linefollowing algorithm and design the software.
However we have all helped each other in order to ge the best solution
possible.
Line Following
The line following algorithm can be improved but is satisfactory. As it is
now it works fine, meaning it manages the test tracks and does so in a
reasonable amount of time. Improvements can be made in the time it takes
to complete a track.
20
Control theory
Control theory is not an easy field as such, this is also the first time that we
have actually implemented any control design. This has resulted in several
rookie mistakes which in turn has resulted in a greater understanding for
the theory.
Modelling
Modelling has proved to be both important and problematic. The first problem we ran into was to producing a model which we never really completed
and the second was to understand all the aspects of the NXT-GS model.
All in all it has been a successful experiment. The time we have spent on
this project has been a bit more than budgeted in form of credits but the
workload has been tolerable. We are pleased with our result and we have
learned a lot along the way.
21
6
Appendix
Software
During the course of this project a number of different matlab m-files and
c-code files were produced. Their functions are described in this chapter
and the files themselves can be found in the ZIP archive accompanying this
report. Copies of the code can also be found at the course home page.
MATLAB files
modell.m - contains the mathematical model of the system and produces
the controller. This is where the weighting matrices are located. fileio.m Generates C formatted code implementing the regulator produced by ’modell.m’ simulator.mdl - This is our simulator implemented in Simulink.
C-code files
tracker.c - This is where the magic happens, in this file the regulator as
well as the line following algorithms are located and run on the actual robot.
balancerar.c - This is our test file where the regulator can be examined
without the line following algorithms effects on the behaviour.
22
Our Model
Use Lagrangian mechanics to derive state equations:
Ek =
m
2
Ek =
L=
Ep = mghm sin(θ)
X mv 2 X Iω 2
Ek =
+
2
2
!2
2πr(φ˙L + φ˙R )
Iθ θ˙2 Iα r2 ˙
+
+
(φL − φ˙R )2
2
2
2 b2
π2 r2 m
Iθ θ̇2 Iα r2
(φ̇L − φ̇R )2
(φ̇L + φ̇R )2 +
+
2
2
2b2
(16)
(17)
(18)
(19)
L = Ek − Ep
(20)
π2 r2 m
Iθ θ̇2 Iα r2
(φ̇L + φ̇R ) +
+
(φ̇L − φ̇R )2 − mghm sin(θ)
2
2
2b2
(21)
Lagrange equations: (P.H. p.165)
∂L
d ∂L
−
= Qx
∂xi dt ∂ ẋi
(22)
In our case we get(assuming Iα and Iθ are constant)
∂L
∂L
=
=0
∂φL
∂φR
(23)
∂L
= −mghm cos(θ)
∂θ
(24)
∂L
Iα r2
= π 2 r2 m(φ̇L + φ̇R ) + 2 (φ̇L − φ̇R )
b
∂ φ̇L
(25)
∂L
Iα r 2
= π 2 r2 m(φ̇L + φ̇R ) − 2 (φ̇L − φ̇R )
b
∂ φ̇R
(26)
∂L
(27)
= Iθ θ̇
∂ θ̇
on the right side ot the equation the generalized forces are described. Here
we have the torque and the viscous friction in the motor.
Qθ = −β(τL + τR )
(28)
QL = τL − bφ φ̇L
(29)
23
QR = τR − bφ φ̇R
(30)
we define some constants to simplify the calculations:
Ka = π 2 m +
Iα
b2
(31)
Kb = π 2 m −
Iα
b2
(32)
with those constants defined we get:
Qθ = Iθ θ̈ + mghm cos(θ)
(33)
QL = r2 (φ̈L Ka + φ̈R Kb )
(34)
QR = r2 (φ̈L Kb + φ̈R Ka )
(35)
τL − bφ φ̇L = φ̈l Ka r2 + φ̈R Kb r2
(36)
τR − bφ φ̇L = φ̈l Kb r2 + φ̈R Ka r2
(37)
−β(τL − τR ) = Iθ θ̈ + mghm cos(θ)
(38)
=⇒
(39)
this gives
φ̈R = −φ̈L
bφ
τL
Ka
−
φ̇L +
2
Kb
Kb r
Kb r2
(40)
φ̈L = −φ̈R
bφ
Ka
τR
−
φ̇R +
2
Kb
Kb r
Kb r2
(41)
mghm
cos(θ) − βτL − βτR
Iθ
(42)
θ̈ = −
(40) −→ (41)
φ̈L =
1−
1
1
2
Kb
Ka
Ka bφ
bφ τR τL Ka
φ̇L
− φ̇R 2 + 2 −
2
Kb r
r
r
Kb r2
(43)
Kb
φ̈R = −φ̈L
bφ
Ka
τL
−
φ̇L +
2
Kb
Kb r
Kb r2
24
(44)
θ̈ =
−mghm cos(θ
− βτL − βτR
Iθ
(45)
and this leads us to the differential eqiations describing the system in our
coordinates.
Ka bφ
Kb
Ka
φ̇L
(46)
φ̈L = 2 a
− φ̇R bφ + τR − τL
r (Kb − Ka2
Kb
Kb
φ̈R = φ̇L
bφ
Ka
− 2 2
−
2
Kb r2
r (Kb − Ka
θ̈ = −
+ φ̇R
bφ
2
r (Kb2 −
Ka2
−
mghm
cos(θ) − βτL − βτR
Iθ
τR
2
r (Kb2 −
Ka2
+
τL
Kb r2
(47)
(48)
It should be noted that
• It is quite a big assumption that Iα is constant and one that probably
does not hold. One possible way to handle this is to to make several
different regulators for different Iα that are valid in different regions
of θ
• It is important to remember that we have not used this model and
have thus not validated it. This means it might exist errors in the final
expressions. Howevere we think that it is correct and the problem is
that it not the best choice of coordinates.
NTXWay-Gs Model
We have used the state space equations of the NXTWay-Gs model. Here
the derivation will follow. This derivation can also be found in the documentation for the NXTWay-Gs model
The figures below combined with the table shows used constants aswell as
choosen coordinates. It also defines all constants
25
26
g
m
R
Jw
M
W
D
H
L
Jψ
Jφ
Jm
Rm
Kb
Kt
n
fm
fw
Gravity acceleration
Wheel weight
Wheel radius
Wheel inertia moment
Body weight
Body width
Body depth
Body height
Distance of the center of mass from the wheel axis
Body pitch intertia moment
Body yaw inertia moment
DC motor inertia moment
DC motor resistance
DC motor back EMF constant
DC motor torque constant
Gear ratio
Friction coefficient between body and DC motor
Friction coefficient betwwen wheel and floor
Table 4: Definitions
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 XXX. If the
direction of two-wheeled inverted pendulum is in x-axis positive direction at
t=0, each coordinates are given as the following.
1
R
(θ, φ) =
(θl + θr ), (θl − θr )
(49)
2
W
Z
Z
(xm , ym , zm ) = ( ẋm dt, ẏm dt, R)
(50)
(x˙m , y˙m ) = (Rθ̇cosφ, Rθ̇sinφ)
W
W
(xl , yl , zl ) = xm −
sinφ, ym +
cosφ, zm
2
2
W
W
(xr , yr , zr ) = xm +
sinφ, ym −
cosφ, zm
2
2
(xb , yb , zb ) = (xm + Lsinψcosφ, ym + Lsinψsinφ, zm + Lcosψ)
27
(51)
(52)
(53)
(54)
The translation kinetic energy T1 , the rotational kinetic energy T2 , the potential energy U are
1
1
1
T1 = m(ẋ2l + ẏl2 + żl2 ) + m(ẋ2r + ẏr2 + żr2 ) + M (ẋ2b + ẏb2 + żb2 )
2
2
2
(55)
1
1
1
1
1
1
2
2
T2 = Jw θ̇L
+ Jw θ̇R
+ Jψ ψ̇ 2 + Jφ φ̇2 + n2 Jm (θ̇l − ψ̇)2 + n2 Jm (θ̇r − ψ̇)2
2
2
2
2
2
2
(56)
U = mgzl + mgzr + M gzb
(57)
The fifth and sixth term in T2 are rotation kinetic energy of an armature in
left and right DC motor. The Lagrangian L hast the following expression
L = T1 + T2 − U
(58)
We use the following variable as the generalized coordinates.
θ : Average angle of left and right wheel
ψ : Body pitch angle
φ : Body yaw angle
Lagrange equations are the following
d ∂L ∂L
= Fθ
−
dt ∂ θ̇
∂θ
(59)
d ∂L ∂L
−
= Fψ
dt ∂ ψ̇ ∂ψ
(60)
d ∂L ∂L
−
= Fφ
dt ∂ φ̇
∂φ
(61)
We derive the following equations by evaluation Eqs (59) - (61)
θ̈(2m+M )R2 +2J +2n2 J)+ ψ̈(M LRcos(ψ)−2n2 Jm )−M LRψ˙2 sin(ψ) = Fθ
(62)
θ̈(M LRcos(ψ)−2n2 Jm )+ψ̈(M L2 +Jψ +2n2 Jm )−M gLsin(ψ)−M L2 φ˙2 sin(ψ)cos(ψ) = Fψ
(63)
φ̈
mW 2
W2
2
2
2
+ Jφ +
(JW + n Jm ) + M L sin (ψ) +2M L2 ψ̇ φ̇sin(ψ)cos(ψ) = Fψ
2
2R2
(64)
28
In consideration of DC motor torque and viscous friction, the generalized
force are given as the following
W
(Fθ , Fψ , Fφ ) = Fl + Fr , Fψ ,
(Fl − Fr )
(65)
2R
Fl = nKi il + fm (ψ̇ − θ̇l ) − fw θ̇l
(66)
Fr = nKi ir + fm (ψ̇ − θ̇r ) − fw θ̇r
(67)
Fψ = −nKt il − nKt ir − fm (ψ̇ − θ̇l ) − fm (ψ̇ − θ̇r )
(68)
where il,r is DC motor current. We cannot use DC motor current directlu in
order to control it because is is based on PWN (voltage) control. Therefore,
we evaluate the relation between current il,r and voltage vl,r using DC motor
equation. The DC motor equation is generally as follows
Lm i̇l,r = vl,r + Kb (ψ̇ − θ̇l,r ) − Rm il,r
(69)
Here we consider that the motor inductance is neglible and is approximated
as zero, Therefore the current is
il,r =
vl,r + Kb (ψ̇ − θ̇l,r )
Rm
(70)
From Equation 70, the generalized force can be expressed using the motor
voltage.
Fθ = α(vl + vr ) − 2(β + fw )θ̇ + 2β ψ̇
(71)
Fψ = −α(vl + vr ) − 2β θ̇ − 2β ψ̇
Fφ =
W
W2
α(vr − vl ) −
(β − fw )φ̇
2R
2R2
(73)
nKt
Rm
(74)
nKt Kb
+ fm
Rm
(75)
α=
β=
(72)
29
State equations of two-wheeled inverted pendulum
We can get 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 second
order terms like ψ 2 . The motion equations Eqn (62)-(64) are approximated
as the following
(2m + M )R2 + 2Jw + 2n2 Jm θ̈ + M LR − 2n2 Jm ψ̈ = Fθ
(76)
M LR − 2n2 Jm )θ̈ + (M L2 + Jψ + 2n2 Jm ψ̈ − M gLψ = Fψ
(77)
W2
1
2
2
mW + Jφ +
(Jw + n Jm ) φ̈ = Fφ
2
2R2
(78)
Equation (76) and Equation (77) has θ and ψ, Equation (78) has φ only.
These equations can be exressed in the form
θ̈
v
θ̇
θ
E
+F
=H l
(79)
+G
v
ψ
ψ̈
ψ̇
r
M LR − 2n2 Jm
(2m + M )R2 + 2Jw + 2njm
E=
(80)
M LR − 2n2 Jm
M L2 + Jψ + 2n2 Jm
β + fw −β
(81)
F =2
−β
β
0
0
(82)
G=
0 −M gL
α
α
H=
(83)
−α −α
I φ̈ + J φ̇ = K(vr − vl )
(84)
1
W2
I = mW 2 + Jφ +
(Jw + n2 Jm )
2
2R2
(85)
J=
W2
(β + fw )
2R2
(86)
W
α
2R
(87)
K=
30
Here we consider the following variable x1 , x2 as state, and u as input (x2
indicates transpose of x)
x1 = [θ, ψ, θ̇, ψ̇]T
(88)
x2 = [φ, φ̇]T
(89)
u = [vl , vr ]T
(90)
Consequently, we can derive state equations of two-wheeled inverted pendulum from Equations (79) - (87),
ẋ1 = A1 x1 + B1 u
ẋ2 = A2 x2 + B2 u


0
0
1
0
0
0
0
1 

A1 = 
0 A1 (3, 2) A1 (3, 3) A1 (3, 4)
0 A1 (4, 2) A1 (4, 3) A1 (4, 4)


0
0
 0
0 

B1 = 
B1 (3) B1 (3)
B1 (4) B1 (4)
0
1
A2 =
0 −J/I
0
0
B2 =
−K/I K/I
(91)
(92)
(93)
(94)
(95)
A1 (3, 2) = −gM LE(1, 2)/det(E)
(96)
A1 (4, 3) = gM LE(1, 1)/det(E)
(97)
A1 (3, 3) = −2[(β + fw )E(2, 2) + βE(1, 2)]/det(E)
(98)
A1 (4, 3) = 2[(β + fw )E(1, 2) + βE(1, 1)]/det(E)
(99)
A1 (3, 4) = 2β[E(2, 2) + E(1, 2)]/det(E)
(100)
A1 (4, 4) = −2β[E(1, 1) + E(1, 2)]/det(E)
(101)
31
B1 (3) = α[E(2, 2) + E(1, 2)]/det(E)
(102)
B1 (4) = −α[E(1, 1) + E(1, 2)]/det(E)
(103)
det(E) = E(1, 1)E(2, 2) − E(1, 2)2
(104)
32
Simulator Setup
33
References
[1] Yorihisa Yamamoto, NXTway-GS Model-Based Design - Control of
self-balancing two-wheeled robot built with LEGO Mindstorms NXT.
http://www.mathworks.com/matlabcentral/fileexchange/19147m/,
2009
[2] Lego, Lego Mindstorms NXT homepage, http://mindstorms.lego.com,
2009
[3] Lego,
Press
release
for
Lego
mindstorms
next,
http://www.lego.com/eng/info/default.asp?page=pressdetail&contentid
=17278&countrycode=2057&yearcode=&archive=false, 2009
34