Embedded Control SystemsLegoWay

Simon BerthilssonAnders DanmarkUlf Hammarqvist

Hanna NygrenVasilij Savin

2009-10-18

Contents

1 Background 31.1 Goal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2 State of the art . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Problem solution 32.1 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 Model design . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2.1 Controller design . . . . . . . . . . . . . . . . . . . . . 32.2.2 State space equations . . . . . . . . . . . . . . . . . . 5

2.3 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3.1 Lego Mindstorms Nxt . . . . . . . . . . . . . . . . . . 62.3.2 Sensors and Actuators . . . . . . . . . . . . . . . . . . 6

2.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.4.1 MatLab . . . . . . . . . . . . . . . . . . . . . . . . . . 62.4.2 SimuLink . . . . . . . . . . . . . . . . . . . . . . . . . 72.4.3 RobotC . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Implementation 73.1 Mathematical model . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.1 NXTway-GS model . . . . . . . . . . . . . . . . . . . 103.2 Deriving the state space equations . . . . . . . . . . . . . . . 12

3.2.1 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . 123.2.2 LQG controller . . . . . . . . . . . . . . . . . . . . . . 13

3.3 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.4 Constructing the robot . . . . . . . . . . . . . . . . . . . . . . 153.5 Algorithm for following a line . . . . . . . . . . . . . . . . . . 16

3.5.1 Sensor reading . . . . . . . . . . . . . . . . . . . . . . 173.5.2 Movement control . . . . . . . . . . . . . . . . . . . . 17

3.6 Experimental results and observations . . . . . . . . . . . . . 173.6.1 PWM issues . . . . . . . . . . . . . . . . . . . . . . . . 18

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 solutionto balance a two wheeled inverted pendulum. The pendulum itself is to beconstructed and controlled by LEGO Mindstorm NXT. The robot shouldalso be able to follow a path (specified by a line drawn on the floor) whilebalancing on two wheels. This should all be done with one gyro sensorand one light sensor. The robot is going to be programmed using RobotCdevelopment platform.

1.2 State of the art

Several solutions to the wheel inverted pendulum (TWIP) problem havebeen published on the Internet, probably the most relevant one is the NXTway-GS where GS is short for Gyro Sensor. The NXTway-GS is based upon aLQG controller and a gyro sensor. The performance of the NXTway-GS isactually quite impressive. Of all inverted-lego-pendulums we have studied,this is probably the one which performs the best and that is in turn probablybecause it utilizes an LQG controller. [1]

2 Problem solution

2.1 Approach

The task of building an inverted pendulum, capable of following line, hasbeen split into several smaller subtasks:

• a line following module

• a controller for balancing

• mathematical model

• controller implementation

• a main control program.

2.2 Model design

2.2.1 Controller design

There are several different types of controllers. One of the most basic isthe PID-controller. The derivation of a PID controller is fairly straightfor-ward and while the control can be rather good, there are limitations. Theinverted pendulum system is an unstable system and requires an accurate

3

and fast control. It is definitely possible to control such a system with aPID regulator but this type of control will be limited in performance.

An LQG (linear-quadratic-Gaussian) controller has the capability to utilizemore known information about the system and create a control which isbetter adapted to each situation. The main drawback of using a LQG con-troller over a PID controller is a more complicated derivation and that itrequires more computational power to run. There are of course several moreregulators as well, the trade-off that it always boils down to is complexityversus performance under certain conditions.

For instance, Model Predictive Controllers are closely related and could mostlikely have been used as well. MPC predicts the systems states a number oftime steps ahead and regulates so that the prediction of the states is withindesired bounds.

The conditions that are to be dealt with in this case are that the system isunstable and non-linear but well known and well estimated by a lineariza-tion around the working point. There are some regulators that can handleunknown systems by means of on-line system identification and other reg-ulators which performs well with non-linear systems. But since the systemin this case is fairly well known it was concluded there was no need to useadaptive controllers. The LQG controller should suffice for project needssince the system is rather well known and can be approximated as a linearsystem for angles within 10 degrees. The LQG controllers are also relativelyeasy to implement and design with Matlab. Matlab has a lot of built infunctions(and help documents) made to aid design of LQG regulators.

When the model has been derived and the states are chosen, the final stepin the design of the LQG controller is to assign proper weights of the dif-ferent states against each other. This stage of the design process is highlyiterative and some experimentation may be needed in order to produce awell behaving controller.

The construction of an LQG controller requires a mathematical model ofthe controlled system. In the ideal case you can measure every interestingproperty of a system and calculate the control signals with a feedback gain.Since we can only measure angular speed of the body and angular positionof the wheels we need some kind of estimate of the remaining states. Acommonly used way of estimating such states in control theory is using anobserver. This project uses a Kalman filter, since it is an optimal linearobserver. 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 necessaryto get perfect descriptions of the noises during the first attempts since sat-isfying results can be achieved anyway.

In order to be able to simulate the system reliably a mathematical descrip-tion of the non-linear system is needed. Obviously with a higher degree ofaccuracy a better correlation between simulations and reality is obtained.However, in order to get a result, a linearised model can be used insteadof a non-linear model. In the end, it will come down to a tweaking processon the actual physical implementation anyway so as long as the linearisedmodel is accurate in the region of interest it’s an acceptable approach.

The controller is usually derived as dynamic system, while microprocessorsoperate with discrete values. This difference should be accounted for. Thesimplest way of doing this is to simply ignore the problem and assume thatthe sampling rate is sufficiently fast in relation to the speed with which thesystem reacts. When this assumption does not hold - the system is fasterthan sampling rate - the regulator has to be sampled. In MATLAB Thiscan be achieved with the command ’c2d’. It is important to remember thatthis can affect the stability of the system and that some additional tuningmay 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 oncreating a vector space of the states of the system in such a way that everypossible state the system can assume may be described by a vector in thisspace. Because of this it is important to choose state variables in such a waythat the vector space becomes linearly independent. There are a few otherimportant 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 reachedduring some amount of time.

The state space form of model can be described by a set of matrix equation

x = Ax +Buy = Cx +Du

(1)

where x is the vector of state variables and u - the vector of control signals

5

2.3 Hardware

2.3.1 Lego Mindstorms Nxt

Lego Mindstorms Nxt is a programmable robotics kit released by Lego inJuly 20061. The main component is Robot Computer Core, which is alsocalled the Brick. It has four sensor ports for input and three motor ports foroutput. All ports peripherals are connected using RJ12 cables. The mainCPU is a 32-bit ARM processor which runs at 48 MHz. Users can uploadprograms to the robot using USB cable or Bluetooth. The brick firmwareis released as open source. Due to the open source license there exists anumber of different unofficial languages, RobotC, Lejos, NXC and NBC toname a few. This has also led to the development of third-party devicessuch as the HiTechnic Gyro Sensor.

2.3.2 Sensors and Actuators

In this project we will utilize three different sensors. Rotary encoders in themotors, the HiTecnic gyro sensor and the stock Lego light sensor. In thisproject two Lego DC motors are used as actuators.

Sensor/Actuator Output/Input Unit Sampling Rate (Hz)DC Motor PWM % 500Rotary Encoder Angle degree 1000Light Sensor Integer Value 0-100 n/a 300Gyro: HT Angular Speed deg/sec 300

Table 1: Sensor and actuator data

When designing real-time software it is especially important to make yourprogram predictable. An usual solution is to a have a number of independenttasks that communicate with each other via shared memory or semaphores.One thing to consider is that all tasks should have time to execute. Thereare a number of different algorithms to control schedulability.

2.4 Software

2.4.1 MatLab

Matlab is a popular numerical computing environment. It is used both in theindustry as well as the academic world. Matlab has numerous extensions,called toolboxes, among other cone called Control Systems toolbox. We willuse Matlab and Control Systems Toolbox as our primary environment forcreating our controller.

6

2.4.2 SimuLink

SimuLink is a very flexible and intuitive way of designing systems since ituses a graphical interface. It’s bundled together with Matlab and the pro-grams are closely linked. Matlab functions can be ran from within SimuLink,and it can import/export to Matlab’s workspace. This makes it ideal forsimulations and visualising the control structure.

2.4.3 RobotC

ROBOTC is a powerful C-based programming language with a Windowsenvironment for writing and debugging programs, and the only programminglanguage at this level that offers a comprehensive, real-time debugger. Wewill use RobotC for programming and debugging our robot.

3 Implementation

A mathematical model of a non-flexible body is preferred since flexible dy-namics is much more complex than non-flexible. Secondly, the centre ofmass preferably should be centred over the wheel axis. This will help thebalancing and facilitate the derivation of the moments of inertia.

3.1 Mathematical model

A successful regulation requires a mathematical model that is accurateenough for the specific needs of the control system. Initially, an attemptwas made to model the system with Lagrangian dynamics. The generalizedcoordinates were chosen according to: φl, φr and θ where φl represents theangle of the left wheel, φr the angle of the right wheel and θ the tilt angleof 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α The moment of inertia around the α-axisIθ The moment of inertia around the θ-axis.m The mass of the robot,τR,L the respective torques of motor R and Lhm The height from the wheel axis to the centre of massg The gravitational acceleration constantr The wheel radiusb 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 deriva-tion: see appendix):

φL =Tb

r2(T 2B − T 2

A)(φL

TAb

TB− φRb+ τR − τL

TATB

) (2)

φR = φL(−TA

r2(T 2B − T 2

A)− b

TBr2)+φR

b

r2(T 2B − T 2

A

− τRr2

(T 2B−T 2

A)+τLTBr2

(3)

θ =−mghm

Iθcos(θ)− λτL − λτR (4)

8

TA = π2m+Iαb2

(5)

TB = π2m− Iαb2

(6)

This model was never implemented or tested. It turned out that derivingsuch 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 tobe problematic since it changes with the tilt angle θ. The proposed solutionto this problem was to use calculate a table of Iα for different values θ, andthen use a look-up table with the closest corresponding value on θ but thiswas never implemented.

At this point in deriving the model we realized it would take us too long tofinish. So instead of deriving our own model we choose to carry on with anexisting model, the one found in the NXTway-GS project, which is knownto work.

9

3.1.1 NXTway-GS model

The NXTway-GS model use three generalized coordinates (θ,φ,ψ) which de-scribes average wheel angle, yaw angle and tilt angle respectively. See Figure2. The used constants can be found in Table 3 .

Figure 2: NXTway-GS Model with generalized coordinates

10

Constant value Unit Descriptiong=9.82 [m/s*s] Gravity accelerationm=0.018 [kg] Wheel massM=0.584 [kg] Body massW=0.16 [m] Body widthD=0.05 [m] Body depthH=0.16 [m] BodyL=H*2/3 [m] Distance to the centre of

mass from wheel axleR=0.031 [m] Wheel radiusJW=m*R*R/2 [kg*m*m] Wheel moment of inertiaJψ=M*L*L/3 [kg*m*m] Body pitch moment of inertiaJφ=M*(W*W+D*D)/12 [kg*m*m] Body yaw moment of inertiaJm=1e-5 [kg*m*m] DC motor inertia momentRm=6.69 [ohm] DC motor resistanceKb=0.468 [V sec/rad] DC motor back EMF constantJt=0.317 [Nm/A] DC motor torque constantn=1 Gear ratiofm=0.0022 Friction coefficient between

body and DC motorfw=0 Friction coefficient between

body and floor

Table 3: Used constants

11

Using these constants together with Lagrangian dynamics yields the follow-ing motion equations.

θ(2m+M)R2 + 2J + 2n2J) + ...

ψ(MLRcos(ψ)− 2n2Jm)−+MLRψ2sin(ψ) = Fθ(7)

θ(MLRcos(ψ)− 2n2Jm) + ψ(ML2 + Jψ + 2n2Jm)− ...−MgLsin(ψ)−ML2φ2sin(ψ)cos(ψ) = Fψ

(8)

φ(mW 2

2 + Jφ + W 2

2R2 (JW + n2Jm) +ML2sin2(ψ))

+ ...

+2ML2ψφsin(ψ)cos(ψ) = Fψ(9)

3.2 Deriving the state space equations

Using the mathematical model derived previously together with a first ordermodel of the DC motor and our constants, we can derive our final systemdescription on state space form. The result is on the form:

x = 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 theDC motors. We tried different sampling rates from 3-10 ms and eventuallywe just chose 5 ms.

3.2.1 Kalman filter

With a model in Matlab it is fairly simple to make an observer. All that isneeded now are the noise estimate matrices. We chose the following matrices.

Q =(Q1 00 Q1

)R =

Q1/100 0 00 R1 00 0 R1

Q1 = 8.726646R1 = 0.034907

(11)

The value on Q is a design parameter that if set low makes the Kalmanfilter respond faster (at the cost of noise sensivity) and if set high makesit resistant to noise (but slow). The values of R were determined throughestimating 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 inthe gyro sensor.

12

3.2.2 LQG controller

In order to calculate the optimal feedback controller for the LQG controllertwo weighting matrices are needed. One matrix for the states (Qx) andanother for the inputs(Qu). We chose the following matrices:

Qx =

104 0 0 0 0 00 104 0 0 0 00 0 103 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1

(12)

Qu =(

1 00 1

)(13)

The values chosen are a result of an iterative process using simulations wherethe initial values are simply guessed and then tweaked until desired dynamicsare achieved. The values chosen puts a lot of weight on the states that makesthe regulator follow the references on position, velocity and leaning angle.

3.3 Simulation

Once a model was chosen we started implementing a simulator in SimuLinkfor testing our regulators. Simulating the linearised model was not so hardbut we never actually managed to simulate the non-linear system in spiteof days spent on troubleshooting. The problems were partly due to de-riving expressions for the complex second order derivatives that could beimplemented, as well as problems with SimuLink and MatLab itself. It wasdecided that it is better to spend time on the actual physical robot andbe satisfied with simulating with a linearised model. Below is a simula-tion(Figure 3) where the robot is instructed to go forward and turn, ie areference 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 onthe next page in Figure 4.

14

Figure 4: A detailed figure for (θ) where the robot is instructed to go forwardand turn, ie a reference change on both position (θ) and turning (φ).

Note that the robot actually goes backward before it can forward to it’s setreference. This because the robot has to lean forward in the direction it’sinstructed to move. In order to move the body has to “fall“ with the wheelskeeping up (The transient in ψ shows the same dynamics but measured inroational velocity).

3.4 Constructing the robot

When designing our robot we had a few things to consider. A rigid physicalmodel 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 experimentswith balancing a stick in a hand, it was discovered that it was easier tobalance a longer stick. This design idea was dropped later, since the periodfor the oscillation for a pendulum increased when the length increases. Thismeans that a higher motor speed is needed with taller bodies and the motorscould not provide the speeds required for our tall design. So the robot wasrebuilt to have a lower profile. Some attention was given to aligning thecentre of mass directly above the wheel axis in order to make for a morenaturally stable system and to accommodate certain model assumptions.No consideration has been taken to the sensors or their placement in themodel but their masses are small enough to safely be ignored assuming theyare placed with some care.

Figure 5: Front and rear view of the robot.

1 NXT-brick2 Motor3 Infrared sensor4 Gyro sensor

3.5 Algorithm for following a line

A line following algorithm is an easy problem to grasp. Firstly, some mea-surements 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 measure-ments are obtained from a stock Lego Mindstorms Light Sensor. Given thatwe have only one sensor and limited computational resources, it has beendecided to use a relatively simple line following algorithm. The algorithminvolves 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 representingthis called OnTrack is set to true. This thread also outputs some debuginformation 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 correc-tive action means that the robot turns on the spot to search to the side whereit last found the line. If the line is not found within some interval, the robotturns to search the other direction and repeats this until the track is foundagain. Control is being performed by inputting new reference values for theregulator, which are later on taken into account by the balancing functionThe line follower algorithm can be seen as a disturbance in the balancingalgorithm as it changes the reference values. Some tuning was needed whichconcerned 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 toreach the set reference point. Obviously there’s room for improvement inthe algorithm. An idea we never had time to investigate was some kind ofcurvature fitting. Using the a number of curve edges it should be possibleto 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 scriptfileio.m has been created. It runs after modell.m reads relevant data andoutputs state space equations for the observer, feedback and reference calcu-lation in RobotC syntax. This then pasted into the RobotC IDE, compiledand run within seconds.

We have encountered several difficulties associated with implementing theRobotC code on the NXT brick. The most significant hardware problemencountered was a problem that occurs when a motor is connected to PortC on the brick. Switching to Ports A and B solved this problem. The symp-toms of this configuration are that the output signals to the motors simplyare set to zero for a short period of time making the robot fall.

Another annoying problem was that sometime it was impossible to uploadnew 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 wasfar from perfect and the differences from C are numerous and significant.It caused some delays in development. Once such nuances have been mas-tered programming became quite straight forward. Our standard solutionto unexpected behaviour from the brick was to upload the firmware againand every time it was not a programming error this solved the problem.

The accumulated wisdom can be summarised this way: When you can’tprogram the robot over Bluetooth, upload the firmware again. If programsdoes not run, upload the program again. When the robot fails to executethe program, correctly re-upload the firmware. Always make sure to branchthe code to prevent loss of valuable data. Check your constants for exampledo not mix Jφ with Jψ. Probably using CVS for this project might havebeen 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 inRobotC we could only control the motors with PWM. We had to convert outfeedback voltage to PWM. In order to convert a voltage to PWM you need todivide with the maximum possible voltage. The maximum possible voltagevaries with battery power and a simple formula to calculate the currentmaximum voltage was needed. To do this we needed a measurement ofmotorspeed versus motorvoltage. From the NxtWay-Gs we got the followingfigure:

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 batteryvoltage(as an integer 8400 meaning 8.4 volts). This meant we had to mod-ify the voltage equation. This is the equation implemented in our controlprogram.

Vmax = 0.0010077 ∗ Vbattery − 0.633 (15)

To convert a voltage to PWM we just have to divide the voltages our con-troller 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 sensordata. The controller then calculates control signals for the motors using theLQG feedback gain and reference values.

19

FollowLine

This task contain the line following algorithm. It uses different commandsfor setting reference values in order to steer the robot.

GetLightReading

This task read the light sensor and decides whether the robot is on or offthe 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 followreference line in an reasonably quick manner. It actually balances betterthan we expected it to but we have produced several controllers with dif-ferent degrees of errors in them during this course. It is difficult to knowwhat performance that can be expected and we have believed that we havereached the best possible regulator a few times but then, by discovering andcorrecting errors, improved it dramatically. It turns out that the controlleris also sensitive to scaling issues in the state variables, meaning our regulatorbehaved better when we managed to implement radians and actual PWMvalues instead of using degrees and just making sure that the resulting out-put lies within the permitted interval. The controller is also very sensitiveto model errors. This is not at all unexpected since the regulator strives tocontrol the model on which it is based, things will go wrong if this differsmuch from the system the controller is implemented on and thus actuallytrying to control.

Work Distributions

During our work on this project each project member had different main re-sponsibilies. Anders was responsible for the design of the regulator. Hannawas in charge of linear simulation. Simon was responsible for model deriva-tion. Ulf implemented the nonlinear model. Vasilij was responsible for thelinefollowing algorithm and design the software.

However we have all helped each other in order to ge the best solutionpossible.

Line Following

The line following algorithm can be improved but is satisfactory. As it isnow it works fine, meaning it manages the test tracks and does so in areasonable amount of time. Improvements can be made in the time it takesto complete a track.

20

Control theory

Control theory is not an easy field as such, this is also the first time that wehave actually implemented any control design. This has resulted in severalrookie mistakes which in turn has resulted in a greater understanding forthe theory.

Modelling

Modelling has proved to be both important and problematic. The first prob-lem we ran into was to producing a model which we never really completedand 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 onthis project has been a bit more than budgeted in form of credits but theworkload has been tolerable. We are pleased with our result and we havelearned a lot along the way.

21

6 Appendix

Software

During the course of this project a number of different matlab m-files andc-code files were produced. Their functions are described in this chapterand the files themselves can be found in the ZIP archive accompanying thisreport. 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 producesthe controller. This is where the weighting matrices are located. fileio.m -Generates C formatted code implementing the regulator produced by ’mod-ell.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 aswell 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 examinedwithout the line following algorithms effects on the behaviour.

22

Our Model

Use Lagrangian mechanics to derive state equations:

Ep = mghmsin(θ) (16)

Ek =∑ mv2

2+∑ Iω2

2(17)

Ek =m

2

(2πr(φL + φR)

2

)2

+Iθθ2

2+Iα2r2

b2(φL − φR)2 (18)

Ek =π2r2m

2(φL + φR)2 +

Iθθ2

2+Iαr

2

2b2(φL − φR)2 (19)

L = Ek − Ep (20)

L =π2r2m

2(φL + φR) +

Iθθ2

2+Iαr

2

2b2(φL − φR)2 −mghmsin(θ) (21)

Lagrange equations: (P.H. p.165)

∂L

∂xi− d

dt

∂L

∂xi= Qx (22)

In our case we get(assuming Iα and Iθ are constant)

∂L

∂φL=

∂L

∂φR= 0 (23)

∂L

∂θ= −mghmcos(θ) (24)

∂L

∂φL= π2r2m(φL + φR) +

Iαr2

b2(φL − φR) (25)

∂L

∂φR= π2r2m(φL + φR)− Iαr

2

b2(φL − φR) (26)

∂L

∂θ= Iθθ (27)

on the right side ot the equation the generalized forces are described. Herewe 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 = π2m+Iαb2

(31)

Kb = π2m− Iαb2

(32)

with those constants defined we get:

Qθ = Iθθ +mghmcos(θ) (33)

QL = r2(φLKa + φRKb) (34)

QR = r2(φLKb + φRKa) (35)

this gives

τL − bφφL = φlKar2 + φRKbr

2 (36)

τR − bφφL = φlKbr2 + φRKar

2 (37)

−β(τL − τR) = Iθθ +mghmcos(θ) (38)

=⇒ (39)

φR = −φLKa

Kb−

bφKbr2

φL +τLKbr2

(40)

φL = −φRKa

Kb−

bφKbr2

φR +τRKbr2

(41)

θ = −mghmIθ

cos(θ)− βτL − βτR (42)

(40) −→ (41)

φL =1

1−(KaKb

)2

1Kb

(φLKabφKbr2

− φRbφr2

+τRr2− τLKa

Kbr2

)(43)

φR = −φLKa

Kb−

bφKbr2

φL +τLKbr2

(44)

24

θ =−mghmcos(θ

Iθ− βτL − βτR (45)

and this leads us to the differential eqiations describing the system in ourcoordinates.

φL =Kb

r2(Kab −K2

a

(φLKabφKb

− φRbφ + τR − τLKa

Kb

)(46)

φR = φL

(− Ka

r2(K2b −K2

a

−bφKbr2

)+ φR

bφr2(K2

b −K2a

− τRr2(K2

b −K2a

+τLKbr2

(47)

θ = −mghmIθ

cos(θ)− βτL − βτR (48)

It should be noted that

• It is quite a big assumption that Iα is constant and one that probablydoes not hold. One possible way to handle this is to to make severaldifferent regulators for different Iα that are valid in different regionsof θ

• It is important to remember that we have not used this model andhave thus not validated it. This means it might exist errors in the finalexpressions. Howevere we think that it is correct and the problem isthat it not the best choice of coordinates.

NTXWay-Gs Model

We have used the state space equations of the NXTWay-Gs model. Herethe derivation will follow. This derivation can also be found in the docu-mentation for the NXTWay-Gs modelThe figures below combined with the table shows used constants aswell aschoosen coordinates. It also defines all constants

25

26

g Gravity accelerationm Wheel weightR Wheel radiusJw Wheel inertia momentM Body weightW Body widthD Body depthH Body heightL Distance of the center of mass from the wheel axisJψ Body pitch intertia momentJφ Body yaw inertia momentJm DC motor inertia momentRm DC motor resistanceKb DC motor back EMF constantKt DC motor torque constantn Gear ratiofm Friction coefficient between body and DC motorfw 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 thelagrangian method based on the coordinate system in Figure XXX. If thedirection of two-wheeled inverted pendulum is in x-axis positive direction att=0, each coordinates are given as the following.

(θ, φ) =(

12

(θl + θr),R

W(θl − θr)

)(49)

(xm, ym, zm) = (∫xmdt,

∫ymdt,R) (50)

( ˙xm, ˙ym) = (Rθcosφ,Rθsinφ) (51)

(xl, yl, zl) =(xm −

W

2sinφ, ym +

W

2cosφ, zm

)(52)

(xr, yr, zr) =(xm +

W

2sinφ, ym −

W

2cosφ, zm

)(53)

(xb, yb, zb) = (xm + Lsinψcosφ, ym + Lsinψsinφ, zm + Lcosψ) (54)

27

The translation kinetic energy T1, the rotational kinetic energy T2, the po-tential energy U are

T1 =12m(x2

l + y2l + z2

l ) +12m(x2

r + y2r + z2

r ) +12M(x2

b + y2b + z2

b ) (55)

T2 =12Jwθ

2L+

12Jwθ

2R+

12Jψψ

2 +12Jφφ

2 +12n2Jm(θl− ψ)2 +

12n2Jm(θr− ψ)2

(56)

U = mgzl +mgzr +Mgzb (57)

The fifth and sixth term in T2 are rotation kinetic energy of an armature inleft 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

dt

∂L

∂θ− ∂L

∂θ= Fθ (59)

d

dt

∂L

∂ψ− ∂L

∂ψ= Fψ (60)

d

dt

∂L

∂φ− ∂L

∂φ= Fφ (61)

We derive the following equations by evaluation Eqs (59) - (61)

θ(2m+M)R2+2J+2n2J)+ψ(MLRcos(ψ)−2n2Jm)−MLRψ2sin(ψ) = Fθ(62)

θ(MLRcos(ψ)−2n2Jm)+ψ(ML2+Jψ+2n2Jm)−MgLsin(ψ)−ML2φ2sin(ψ)cos(ψ) = Fψ(63)

φ

(mW 2

2+ Jφ +

W 2

2R2(JW + n2Jm) +ML2sin2(ψ)

)+2ML2ψφsin(ψ)cos(ψ) = Fψ

(64)

28

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

(Fθ, Fψ, Fφ) =(Fl + Fr, Fψ,

W

2R(Fl − Fr)

)(65)

Fl = nKiil + fm(ψ − θl)− fwθl (66)

Fr = nKiir + fm(ψ − θr)− fwθr (67)

Fψ = −nKtil − nKtir − fm(ψ − θl)− fm(ψ − θr) (68)

where il,r is DC motor current. We cannot use DC motor current directlu inorder 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 motorequation. The DC motor equation is generally as follows

Lmil,r = vl,r +Kb(ψ − θl,r)−Rmil,r (69)

Here we consider that the motor inductance is neglible and is approximatedas 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 motorvoltage.

Fθ = α(vl + vr)− 2(β + fw)θ + 2βψ (71)

Fψ = −α(vl + vr)− 2βθ − 2βψ (72)

Fφ =W

2Rα(vr − vl)−

W 2

2R2(β − fw)φ (73)

α =nKt

Rm(74)

β =nKtKb

Rm+ fm (75)

29

State equations of two-wheeled inverted pendulum

We can get state equations based on modern control theory by linearizingmotion equations at a balance point of NXTWay-GS. It means that weconsider the limit ψ → 0 (sin(ψ) → ψ, cos(ψ) → 1) and neglect secondorder terms like ψ2. The motion equations Eqn (62)-(64) are approximatedas the following

[(2m+M)R2 + 2Jw + 2n2Jm

]θ +

(MLR− 2n2Jm

)ψ = Fθ (76)

(MLR− 2n2Jm)θ + (ML2 + Jψ + 2n2Jm

)ψ −MgLψ = Fψ (77)

[12mW 2 + Jφ +

W 2

2R2(Jw + n2Jm)

]φ = Fφ (78)

Equation (76) and Equation (77) has θ and ψ, Equation (78) has φ only.These equations can be exressed in the form

E

[θ

ψ

]+ F

[θ

ψ

]+G

[θψ

]= H

[vlvr

](79)

E =[(2m+M)R2 + 2Jw + 2njm MLR− 2n2Jm

MLR− 2n2Jm ML2 + Jψ + 2n2Jm

](80)

F = 2[β + fw −β−β β

](81)

G =[0 00 −MgL

](82)

H =[α α−α −α

](83)

Iφ+ Jφ = K(vr − vl) (84)

I =12mW 2 + Jφ +

W 2

2R2(Jw + n2Jm) (85)

J =W 2

2R2(β + fw) (86)

K =W

2Rα (87)

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 pendu-lum from Equations (79) - (87),

x1 = A1x1 + B1ux2 = A2x2 + B2u

(91)

A1 =

0 0 1 00 0 0 10 A1(3, 2) A1(3, 3) A1(3, 4)0 A1(4, 2) A1(4, 3) A1(4, 4)

(92)

B1 =

0 00 0

B1(3) B1(3)B1(4) B1(4)

(93)

A2 =[0 10 −J/I

](94)

B2 =[

0 0−K/I K/I

](95)

A1(3, 2) = −gMLE(1, 2)/det(E) (96)

A1(4, 3) = gMLE(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 ofself-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