White paper applying control theory to the stability of an inverted pendulum robot built mostly from Vex robotic kit parts. Includes MATLAB and OCTAVE code for nonlinear model.

Attribution Non-Commercial (BY-NC)

4.2K views

White paper applying control theory to the stability of an inverted pendulum robot built mostly from Vex robotic kit parts. Includes MATLAB and OCTAVE code for nonlinear model.

Attribution Non-Commercial (BY-NC)

- A New Configuration of Two-Wheeled Inverted Pendulum: A Lagrangian-Based Mathematical Approach
- Self Balancing With Pid Control
- Two wheeled self balancing robot with fuzzy controller
- Thesis - Self Balancing Robot
- Inverted pendulum
- Control of Inverted Pendulum
- Self Balancing Robot
- inverted_pendulum
- Inverted Pendulum Supporting Controls
- Matlab_Accelerating Flight Vehicle Design
- Modeling an Inverted Pendulum
- Wheeled Inverted Pendulum Equations
- Linear Quadratic Regulator (LQR) Controller Design for DC Motor Speed Using Matlab Application
- Building a Two Wheeled Balancing Robot
- Inverted Pendulum
- Inverted Pendulum
- Self-balancing robot
- Self balancing Robot
- InvertedPendulum
- Inverted Pendulum

You are on page 1of 59

Rev 1 , 8/20/07

(Draft)

Author : Vamfun

Vamfun@yahoo.com

1

Table of Contents

1. Introduction

2. Free Body Definitions

3. Force and Moment Equations

4. Nonlinear Model

5. Linear Model

6. Sensor Model

7. Motor Model

8. Motor Controller Dead Zone

9. Motor Friction Model

10. State Propagation

11. Control Law Analysis (tbd)

12. System Engineering (tbd)

Appendices

a. OCTAVE Program Installation Notes

b. MATLAB/OCTAVE Simulation Program

c. MPLAB vex bal_bot source ‘C’ code

d. Tutorial Links

e. Moment of Inertia Calculations

f. Steady-state motor rate vs pwm_cmd data

Change Log

2

1. Introduction

for our First ROBODOX 599 team summer project. The goal of the project

was to give the students modeling tools to design their own pendulum robot.

The tools included MATLAB or OCTAVE(free gnu sw) control system

analysis software, analytical models plus enough control theory to be

dangerous. The project preparation includes several lectures on calculus,

matrix theory, Laplace transform theory, linear feedback control theory and

sensor/processor/motor engineering fundamentals. The project is ongoing as

of 8/20/07.

The first prototype was a Vex kit Bbot which is discussed in Vex forums

http://www.vexforum.com/showthread.php?t=1593

www.youtube.com/watch?v=4loT_Xfhvbk

This one shows effects with and with out encoder feedbacks.

www.youtube.com/watch?v=2UqIXdxdQBY

The following sections are an attempt to document the equations used in

Matlab/OCTAVE and certainly use a lot of assumptions to get going. I ask

the reader to provide comments, add content or corrections to improve the

quality this white paper. The author is fairly new to both Vex and

Matlab/OCTAVE so you may see some strange things as I stumble my way

around.

Thanks,

3

Vamfun

Retired Lockheed Avionic and Control Systems Engineer

Mentor Robodox Team 599

4

2. Free Body Definitions

Right side view Rear view

Y axis

Y axis

xcg, ycg

xw,yw

Z axis

X axis

m=mass of body

N g=gravity

mw=mass of wheel

U r=radius of wheel

l=distance from wheel axle to body cg along body y axis

C xw,yw position of wheel axle centroid

xcg, ycg position of body cg

Fxg, Fyg ground forces

m*g N,U forces on axle

C torque applied to axle by motor

C Ө=pitch angle positive ccw about z axis

Өw=wheel angle positive ccw about axle z axis

Өm= Ө-Өw= relative rotation angle + cw about axle

Izb=principle moment of inertia about cg body z axis

U

N Izw=principle moment of inertia about cg wheel z axis

Fyg

Fxg

5

3. Force and Moment Equations

Now apply Newton’s laws by equating accelerations to the sum of

forces/mass and equating angular accelerations to the sum of moments/Inertia

for the body and wheels. Only the symmetric components of the two

wheels are needed and we can consider the problem to have only one wheel

with twice the mass and moment of inertia equal to the sum of left and right

wheel moments and the torque to be the sum of motor torques generated by

motors on each wheel.

Important: the moments must be summed about the cg since the free bodies

are both translating and rotating.

We will use the notation x_d, x_dd to mean the first and second derivative of

x with respect to time where x is any variable.

Body

1) m * xcg_dd = U :inertial axes

2) m* ycg_dd = N – m*g :inertial axes

3) Izb* Ө_dd = l*(U*cos Ө + N*sin Ө) + C :body axes

Note: Summing moments in body fixed axes allows us to keep principle axis

Izb fixed as body rotates.

Wheel

5) mw*yw_dd = Fyg – N – mw*g :inertial axes

6) Izw* Өw_dd = r*Fxg – C :body axes

8) xw_d = - r*Өw_d

9) xw_dd = - r*Өw_dd

11) ycg = yw + l*cosӨ

6

12) xcg_d = xw_d – l*cosӨ*Ө_d

13) ycg_d = yw_d – l*sinӨ*Ө_d

15) ycg_dd = yw_dd - l* cosӨ*Ө_d* Ө_d – l*sinӨ*Ө_dd

Normally, Fyg would be a function of wheel tire states (i.e. yw and yw_d )

and this force equation would give us a final relation. However, we will

assume that the wheels are always on the ground so our final constraint

equations are :

16) yw =r

17) yw_d = 0

18) yw_dd = 0

4. Nonlinear Model

The inverted pendulum problem only requires two independent variables to

completely describe the state at any time. Let us choose as our state pair,

[Өw , Ө] , the angular position of the wheel and body and proceed to

eliminate all other variables to get two nonlinear differential equations

relating angular state accelerations to angular states , state rates and input

torque C. I.e. we want equations to look like:

Ө_dd = g2( Өw , Өw_d , Ө , Ө_d ,C)

yw_dd , xcg_dd and ycg_dd from force equations then use the resulting

equations to solve for N , U and Fxg for substituting into the moment

equations 3 and 6.

d21* Өw_dd + d22* Ө_dd = e2

7

where

d11 = Izw + (mw +m)*r^2

d12= m*r*l*cos Ө

d21=m*r*l*cos Ө rev1

d22 = Izb + m*l^2 rev1

and

e2 = m*l*g* sin Ө +C

20) Ө_dd = (-d21*e1 + d11*e2)/det

where det=d11*d22-d12*d21

Lets go another step and use relative rotation angle Өm as a state variable

in place of Өw. This will save an output equation later on.

Define

21) Өm = Ө - Өw

The sign chosen will produce a positive x movement for a positive motor

movement if Ө is held constant. I.e looking normal to the right wheel ,

positive motor will be in clockwise direction. Өm is the motor angle after

gearing is taken into account.

********************************************************

22) Өm_dd = ((-d21- d22)*e1 + (d11+d12)*e2)/det

23) Ө_dd = (-d21*e1 + d11*e2)/det

8

where d11 = Izw + (mw +m)*r^2

d12= m*r*l*cos Ө

d21=m*r*l*cos Ө

d22 = Izb + m*l^2

det = d11*d22 – d12*d21

e1 = m*r*l*sin Ө*( Ө_d) ^2 - C

e2 = m*l*g* sin Ө +C

******************************************************

5. Linear Model

The nonlinear model is used for simulation but we need a linear model to

apply linear control theory. We want the standard state space model

=[ x1 , x1_d , x3 , x3_d] T

=[ x1 , x2 , x3 , x4 ] T

x_d=[x1_d , x2_d, x3_d, x4_d ] T

and u = δC

Here the states are all understood to be perturbations about a nominal trim

state. We will use the vertical balance trim state where at time zero

Ө = 0 , Ө_d=0. , Ө m= 0 , Өm_d=0. , C=0.

We could derive the Jacobian of x_d=g(x, C) but we know that making the

following substitutions into our nonlinear equations will give us the same

results:

sin Ө = δ Ө

cos Ө= 1

Ө_d* Ө_d= δӨ_d * δӨ_d =0 product terms of small numbers 0

sin Ө*sinӨ= δӨ* δӨ = 0

9

From eq 22) and 23)

27) x3_dd = (-p21*e1 + p11*e2)/detp

d11,d21,d21,d22,e1,e2 respectively.

p12= m*r*l

p21=m*r*l

p22 = Izb + m*(l)^2

detp = p11*p22 – p12*p21

q1 = - u

q2 = m*l*g* x3 +u

for the state rates as a function of x and u.

x1_d = x2

x2_d = (m*l*g*(p11+p12)/detp) *x3 + (p21+p22 +p11 + p12)/detp *u

x3_d = x4

x4_d = (m*l*g*p11/detp)*x3 + (p21 + p11)/detp *u

A is a 4x4 matrix , B is a 4x1 and the non zero elements of A and B are:

A B

a12 =1.

a23 = m*l*g*(p11+p12)/detp b2 = (p21+p22 +p11 + p12)/detp

a34 =1.

a43= m*l*g*p11/detp b4 = (p21 + p11)/detp

10

6. Sensor Models

The pendulum problem uses three types of sensors. Tilt angle rate gyro,

fore/aft body accelerometer and right and left motor position encoders.

The rate gyro and accelerometer are prefiltered with a first order lag to

reduce bandwidth prior to sampling by the processor a/d. Let us define two

new states :

The differential equation for a first order lag with bandwidth k_tau is

Accelerometer :

Rate gyro :

We have x4 already for the rate gyro but we need xacc for the accelerometer.

11

An accelerometer outputs a signal that is proportional to the acceleration

along its sensitive axis. One can conceptually model the internals of an

accelerometer as a small mass suspended by a spring that can only move in

the direction of the sensitive axis. The accelerometer outputs a signal that is

proportional to the force on the spring which in turn is proportional to the

acceleration of the small mass plus gravity. If the accelerometer is stationary

and oriented so the sensitive axis is along the direction of the support force

needed to counteract its weight the output acceleration will be equal to

gravity. So we need the kinematic acceleration at the location of the

accelerometer plus the gravity component opposite to the weight vector.

If the accelerometer is located on the body at a distance ‘la’ from the wheel

center with its sensitive axis normal to the body along the body +x direction

then

where

The first two terms are components of wheel centered inertial axes

acceleration in body x axis and the third term is the tangential acceleration at

a distance ‘la’ relative to the wheel center.

g*sin(Ө)

12

Only the accelerometer model needs linearization. Substituting the linear

equivalents into eq 31) gives

la*(p21 + p11) )*u/detp

include sensor states gives

x=[x1,x1,x3,x4,x5,x6]T . x_d=[x1_d,x2_d,x3_d,x4_d,x5_d,x6_d] T

A(5,5) = -k_tau_5

B(5) = ( r*(p22 + p12) –la*(p21 + p11) )/detp*k_tau_x5

A(6,4) = k_tau_x6

A(6,6) = - k_tau_x6

B(6) = 0

13

7. Motor Model

Figure 1 shows a simplified block diagram of the pendulum control system.

pwm C

a/d Vex Microprocessor

Gain d/a Dead Motor

Zone Controller

Gain Motor gear

Body/

Wheel

Left+Right Model

X

Sensors

Control System

We need to define the relationship between the pwm motor command generated by our

microprocessor and the torque on the body/wheel ‘C’.

Figure 2 further defines the analytical terms used in the following sections for motor ,

controller and friction models

14

+-ctrl_pwm_lim

Vex tq_per_pwm

Processor

Dead zone

width +_dz

motor

speed motor

- +

Body/Wheel Өm_d Gain w Gain back EMF

m_gear tq_per_w torque -

Dynamics

Net Motor Cm

torque

Friction

Cf +

Functions

+

friction

Cs,Cd,Cv

torque

C

Toeal

Torque

Body/Wheel

Dynamics

The Vex motors can be modeled simply if we ignore the motor inductance

and just include the back emf terms.

Define :

relative to neutral (pwm_cmd neutral=0) and having a max range [-127,127].

pwm_cmd_max= 127

15

m_num=number of motors adding to C (2 in this case)

note: pwm_max is best determined from lab tests of your motor.

See appendix F.

tq_max = single motor stall torque (6.5 inlb for Vex motor)

note: See appendix F for lab test data.

= w_max/(pwm_w_max- dynamic_dz)

note: dynamic_dz is defined in section 8 on the Motor Controller Dead

Zone

= tq_max*m_gear*m_num/(pwm_w_max-dz)

= tq_per_pwm/w_per_pwm

tq_per_Өm_d = tq_per_w*m_gear

=pwm_w_max – dz

= pwm_dz for |pwm_dz| < ctrl_pwm_lim

= +ctrl_pwm_lim for pwm_dz>= ctrl_pwm_lim

= -ctrl_pwm_lim for pwm_dz<=-ctrl_pwm_lim

16

Cm = (tq_per_pwm*pwm_dz_lim - tq_per_w*w) or in terms of relative

angle

The Vex motor controller has an electronic dead zone by design to protect the

motor from always running. The vex forum states “The nominal dead band

varies from 1.47ms – 1.55ms but over all temperature and voltage ranges

could vary from 1.36ms – 1.68ms.” Converting this to pwm counts we get a

range of 120.3 -- 140.8 nom and 92.2--174 over range. For simulation

purposes we can approximate this as +- 10 pwm nom about neutral and +-42

pwm about neutral over range.

A dead zone can be constructed by taking the difference between the variable

and the variable limited to +_ dz where dz =half of the total dead zone.

Let pwm_cmd equal the input to the dead zone. The output of the dead zone

would be pwm_dz , the motor command.

if (x>upper_lim) out=upper_lim

elseif (x<lower_lim) out=lower_lim

else out=x

end

17

These are larger than the controller dz and are caused by dynamic and static

friction effects. We determine these experimentally by installing the motors

in the robot to get friction loads as close as possible to operational conditions.

We then increase the pwm_cmd until the motors start to move. The

pwm_cmd value at this point is the static_dz width. As the pwm_cmd is

lowered, the motor continues to run until a value below static_dz is reached

that stops the motor. This value is dynamic_dz and represents the sum of

controller dz and the additional pwm_cmd to overcome dynamic or kinetic

friction torques. We measure these values for each motor in both forward

and reverse directions so we get four numbers for the left and right motors.

We then take the total static dead zone and divide it by two to get the

static_dz and similarly for dynamic_dz for each motor.

Appendix G. shows data taken from the vex balance robot motors.

dynamic_dz=15 (measured)

static_dz= 18 (measured)

Friction comes in many forms.

http://www.20sim.com/webhelp4/library/iconic_diagrams/Mechanical/Frictio

n/Static_Friction_Models.htm

Friction takes the form of static, dynamic and viscous. Static friction is

present only when velocity ,v, of the motor =0 and when v != 0 Coulomb

(dynamic) friction and viscous friction are present. Viscous friction is

typically proportional to v . All terms oppose the motion of the motor. We

will model these as friction torque terms Cs , Cd and Cv .

=0 otherwise.

18

{ 1 if v>0 }

sign(v)={ 0 if v=0 }

{ -1 if v<0 }

C = Cm + Cf

surfaces, the coefficient of sliding friction μd and a moment arm that creates

a torque. Cs_max is also proportional to the normal force , the coefficient of

static friction μs and a moment arm that creates a torque. We can model the

magnitude of these quantities in terms of pwm_cmd units. As discussed

earlier under controller dz topic, we difined three quantities; dz, dynamic_dz

and static_dz. The latter are determined from test results using the vex

motors while installed in the robot. Lets assume that the difference between

static_dz and the controller dz is pwm equivalent units of torque loss due to

static friction. So we define Cs_max in terms of motor torque units as

Cs_max=(static_dz-dz)*tq_per_pwm.

u_ratio is computed from the measured dead zone information or one can just

assume it to be ratio between dynamic and static coefficient of friction.

Appendix G data gives u_ratio = (15-10)/(18-10)=.625 which is reasonable.

w=w_max. Therefore ,

Cv=cv_ratio*Cs_max/w_max.

now we will assume cv_ratio=0 and assume any effects are accounted for

by our calculation of tq_per_w gain.

19

10. State Propagation

We now have equations relating state velocity to states and inputs.

Given an input u(t) and initial state vector x0 we can now find x at any time t

by performing numerical integration. Numerical integration takes many

forms but we will just consider rectangular method here. This is achieved by

assuming that x_d is constant between updates. This assumption leads to a

difference equation by approximating the continuous x_d by the change in x

over a sample time interval T. i.e.

x_d(n)= (x(n+1)-x(n))/T

x(n+1)=x(n) +x_d(n)*T

where T is the update sample rate , x(n) is current state and x(n+1) is the

next state. This is fairly accurate as long as 1/T is about 5 times higher than

the highest eigenvalue (or frequency ) of your system.

x_d(n) is obtained from either the nonlinear or linear state equations derived

above.

We can now simulate any system in state space form with the following

simple pseudo code example for the linear problem.

x= x0; t=0 ; % initialize state vector and time zero

Set Tmax and T % Tmax =maximum simulation time, T= sample interval

While (t<=Tmax) % Iterate problem until max time is reached

u=f(t ) % compute input as function of time

x_d=A*x+B*u % compute state rate x_d(t)

x = x + x_d*T % update state

t=t+T % update time

End while

20

11. Control Law Analysis

Referring to Figure 1, we now need to derive the gain used by the Vex

processor to stabilize the inverted pendulum robot. The process of

obtaining the transfer function between pwm_cmd to the motors and the

sensor outputs involves several steps. First we find the open loop transfer

function X/C between motor torque (C) and the system state variables (X),

then we close the motor back-electromotive-force (back emf voltage loop)

and obtain the new system transfer function X/pwm. We then close the

final loops with constant gains K such that pwm=-K*X where K*X is a

vector = k1*x1 +k2*x2 + … + k6*x6. We can derive these gains using

classical frequency domain bode analysis, Laplace domain root locus analysis

or time domain analysis. In the time domain, we can vary individual gains

or we can manipulate cost function parameters visa modern control theory

optimization technique called LQR (Linear Quadratic Regulator) to match a

desired time response. In this paper, we will use the latter LQR design

technique which is straight forward with the Matlab control system tool box.

The reader should visit the tutorial links on Matlab

http://www.engin.umich.edu/group/ctm/examples/pend/invSS.html#lqr

, the ucsb undergraduate lecture notes on LQR

http://www.ece.ucsb.edu/~hespanha/ece147c/web/lqrlqgnotes.pdf

also see Wikipedia LQR write up at

http://en.wikipedia.org/wiki/Linear-quadratic_regulator .

Once the gains are found using the linear system model, they are further

refined with the nonlinear Matlab simulation and on the robot which will of

course introduce additional nonlinearities and phase lags not modeled. Note,

it is understood that references to Matlab functions also imply Octave

functions since they are identical with few exceptions.

So let’s proceed:

21

x =[ δӨm , δӨm_d ,δ Ө , δӨ_d , x5, x6]T . Recall, that δӨm is the relative

rotation between the body and wheel which is proportional to the encoder

sensor output, δ Ө is the tilt angle relative to vertical and x5 and x6 are the

lagged accelerometer and rate gyro outputs (filtered sensor data). u is the

input torque C= Cm + Cf acting on the body and wheels. For the linear

analysis, we assume the friction torque Cf = zero so u = Cm the motor

torque.

A and B are the linear matrices defining system dynamics given in section 5

and augmented in section 6 with sensor dynamics.

y=[δӨm , 0 , 0 , 0 , x5, x6]T since we only have the three sensors.

matrix, not the torque input.

C= [1 0 0 0 0 0 ;0 0 0 0 1 0; 0 0 0 0 01] and

D= [0 ; 0; 0]

To proceed with Matlab, we must associate the state matrices with a state

space model and name the model for future use.

‘sys’ is the name of the state space. We can define and access data from sys

by using a membership command i.e.

Next, lets add the motor back emf term which creates a torque feedback

proportional to motor speed and change the input command to be pwm_cmd.

and name the new state space ‘sysm’ . We assume that dead zones and

nonlinearities between pwm_cmd and Cm are have no effect in the linear

analysis. So from Fig 3 we know that

22

u = Cm= -tq_per_w*m_gear* Өm_d + tq_per_pwm* pwm_dz_lim

= -tq_per_ Өm_d * Өm_d + tq_per_pwm*pwm_cmd

u = -Km*x + tq_per_pwm*pwm_cmd

= (A-B*Km)*x + B*tq_per_pwm*pwm_cmd

This defines the new A and B matrices for the ‘sysm’ state space. So,

(pwm_cmd=0) eigenvalues

oleig_motor= eig(sysm)

where eig is the Matlab function that outputs an array of eigenvalues for the

sysm.a system matrix.

There is one positive eigenvalue at 6.95 which represents a root in the right

half s plane so we know that the inverted pendulum is an unstable system.

Our design objective is to create a new feedback

pwm_cmd = -K*x

23

and a K which will stabilize this pole and also give us the desired time

response.

We must first define our cost function J by creating weighting factors Q and

R that penalize large amplitude responses in x and u respectively. u in this

case is pwm_cmd.

If we make Q diagonal then the integrand of cost function takes a scalar form

We see that this is always positive and we simply must provide 7 constants

Q=diag[ q11 q22 q33 q44 q55 q66] and r11= wu where wu can be

considered as the relative weighting between control and state variables if we

restrict the q’s to be unity. i.e. Q=diag[1 1 1 1 1 1 ]

associated Riccati equation which we won’t address here and the closed-loop

eigenvalues

cleig_motor = eig(sysm.a-sysm.b*K)

Q=diag[1 1 1 0 0 0] , wu=100 . We didn’t penalize movement in tilt angle

rate or the sensor outputs and we weighted the command heavily to prevent

saturation of the control inputs while still maintaining an adequate response

time for recovering from tilt disturbances.

24

These weightings resulted in

25

APPENDICES

MATLAB with the controls toolbox is the tool of choice for the project but a

student version runs about $100. There is , however, a free great

replacement program which has essentially the same command structure as

MATLAB but not a great graphical user interface (GUI). The program is

called 'OCTAVE' and can be downloaded from the GNU website and comes

with good documentation. Actually, MATLAB tutorials are just as good for

learning the functions.

Go to this website:

http://sourceforge.net/project/showfiles.php?group_id=2888

and download the OCTAVE Forge Windows Package , Release OCTAVE

2.9.13 for Windows Installer , July 26, 2007. After unzipping, install and

change the environment to include the directory you want to use for the

pendulum project. I called mine C:/ ......./octavevex and added the

following to the .octavec environment startup program located at C:\Program

Files\octave2.9.13\share\octave\site\m\startup\octavec on my installation.

The resulting octavec file looked like:

##

## This file should contain any commands that should be executed each

## time octave starts for every user at this site.

addpath("C:/Documents and Settings/cds/My Documents/vex

robotics/octavevex");

Use the editor that comes with the program and generate some test scripts

and save them to your dir.

Start OCTAVE and wait for program to bring a dos command prompt. Type

the name of the test program without quotes i.e. > testprog and you

should see the results in the command window.

A simple test program might be the entry of a 2x2 matrix and performing

some operations.

26

%this is not a function

a=[2 , 1 ; 3 ,5]

b=[1 0 ; .5 3] .

a*b

b*a

note: the separation commas or spaces are acceptable for the elements of a

matrix row

You should of course get different answers for a*b and b*a

Your output should look like

a=

2 1

3 5

b=

1.0000 0

0.5000 3.0000

ans =

2.5000 3.0000

5.5000 15.0000

ans =

2.0000 1.0000

10.0000 15.5000

27

28

B. MATLAB/OCTAVE Simulation Program

Copy and Paste this to a text file and change name and type to

“InvertPend_vex_6state_nl.m”

%File must be saved as "InvertPend_vex_6state_nl.m"

%Requires Matab/Octave Control Systems toolbox

%Program runs with both

%Differences between Matlab and Octave are noted.

%created by Vamfun email:Vamfun@yahoo.com

%Rev 8/20/07

%For a detailed description of the models see the white paper reference:

% "Modeling a Two Wheeled Inverted Pendulum Robot" by Vamfun, Rev 1 8/20/07

function InvertPend_vex_6state_nl

sw_param = 0 ; % =1 if you want parametric runs else =0

%this switch allows us to run multiple values for any parameter.

%To see multiple runs on the same plot comment "hold off" lines in the plot

%section of vex_pendulum_nl.

%Set your variable = param_value in the first line of vex_pendulum_main

if(sw_param == 1)

param = [1 3 7]; %set your row parameter vector here

%this example uses 3 locations of the accelerometer distance 'la'

n_param = max(size(param(1,:))) %count the parameters

for i=1:n_param

'******** new parameter ***************** ',

vex_pendulum_main(param(i)) %call the main function

end %end for

else

vex_pendulum_main('none');

end %end if sw_param

end %end InvertPend_vex_6stat_nl

la=2.54 ;% put default parameter value here

if ~strcmp(param_value ,'none')

la = param_value % Here 'la' is the parametric variable as example

end

' ***************************************'

% ******* Simulation control parameters **************

T=10.; dt=.0185; %T is run time sec, dt is sample time~sec

%Nominal user_routine update for Vex processor is 18.5ms

steps_per_sample =fix(dt/.001) % round to nearest integer relative to .001s

dt_true=dt/steps_per_sample; % this sets the simulation steps per sample

%nominally dt= .0185 and dt_true= .0010278, steps_per_sample=18

%***** a/d parameters *******

sw_A_D_in=1 %Normal =1, set to 0 to bypass a/d effects

N=10 % 10 bit a/d

%1024 cnts with 10 bit a/d

29

sf_encoder=4950/2/pi() %mv/rad =4950mv/rev*rev/6.28rad

sf_az=300 %mv/g

sf_thetadot=2*180/pi(); %mv/rps =2mv/deg_per_s*deg_per_rad

cnts_per_mv=2^N/5000 %cnts/mv

sf=cnts_per_mv*[sf_encoder sf_thetadot sf_az] %cnts/unit

rad=180/pi(); %deg/rad

ozpdyne=(1/0.278)*1.0e-5 %oz/N*N/dyne

cmpin=2.54; %cm per in conversion

gramspoz=28.35; % grams per oz conversion

%**** mass properties *****************

%pendulum dim l_x,l_y,l_z (width,height,depth) z is parallel to wheel axis

g = 980.7; % cm/s**2

l = 2.5*cmpin %cm= in*cmpin , this is the distance to cg of body from axle

l_x=1*cmpin %width of body normal to pivot

l_y=8*cmpin %height of body

l_z=8*cmpin % cm=in*cmpin width of beam parallel to pivot

b_vol=l_x*l_y*l_z%

rho=1.14 % density

M_est=rho*b_vol % estimated mass assuming uniform density

Izb_est=1/12*M_est*(l_x*l_x+l_y*l_y) %est. body principle moment about z axis

%assumes that bottom of body sits on axle.

M=40.5*gramspoz % grams=oz*grams_per_oz

%we can experimentally determine Izb by measuring the period around wheel axis

%(2*pi/Period)^2=g*l*M/Izbaxle , Izbaxle=Izb+l^2*M

%Izb=g*lcg*M*(Period/2*pi)^2-lcg*lcg*M

Period=.714;

Izb=g*l*M*(Period/2/pi())^2-l*l*M

%Wheel radius =1.35in

r=1.35*cmpin %radius in cm

wheel_width= 1.7 %cm

mw_est=rho*3.14*r*r*wheel_width*2 % theory

%measured mass of small wheel=2 oz

mw=(2*gramspoz)*2 %(mass of 2 wheels)

Izw=(.5*mw*r*r)*2 %2.51*2; %sum of left and right wheel principle z axis moments

%assumes uniform disc with mass mw

%********motor/controller parameters************

%Vex motor stall torque 6.5 in lbs, motor no load speed = 100 rpm

pwm_max=127; %Maximum motor command

ulimit=pwm_max %motor pwm cmd limit

u_ctrl_dz=10 %1/2 of total controller pwm deadzone nominal=5 pwm cnts

dz_comp=0 %compensation bias for pwm cmd deadzone.

% This is added to command as a bias to keep command out of dz area.

m_tqmax=6.5*16/ozpdyne*cmpin %motor stall torque in dynes*cm

m_wmax=100*2*pi()/60. %motor no-load speed rpm*2pi/60sec/min=rad/sec

m_gear=1 %torque increase due to gearing (motor rev/wheel rev)

m_num=2 %number of motors

tq_per_pwm=m_tqmax/pwm_max*m_gear*m_num %gain = torque/pwmcmd

tq_per_xmd=m_tqmax/m_wmax*m_gear*m_gear*m_num %gain = torque/xmd

pwm_per_xmd=tq_per_xmd/tq_per_pwm

u_frict_dz=40 %1/2 of total friction equivalent pwm deadzone, nom=40 pwm cnts

30

%we obtain an estimate of this by commanding the motors while installed on

%robot with normal loads. The cmd value that just gets the wheels going is

%u_frict_dz

Cs_max_pwm=u_frict_dz-u_ctrl_dz; %maximum static friction

u_ratio=.7 % ratio between dynamic and static friction coefficients

cv_ratio=.1 % ratio of Cv@max rpm/Cs_max

Cd_pwm= u_ratio*Cs_max_pwm % Max dynamic friction torque

Cv_pwm= cv_ratio*Cs_max_pwm/m_wmax*m_gear %

slope_factor= (u_ratio+cv_ratio)*Cs_max_pwm/pwm_max +1

%slope_factor is a gain that gives max rpm when pwm=pwm_max

%accelerometer parameter xac=(xdd_la+g*theta)/(s/k_tau_5+1)

%accelerometer assumed located 'la' cm from wheel axle

%la=2.54 %cm

k_tau=2 % 1/tau_wo for accelerometer blending with pitch angle

k_tau_x5 = 62.8 % accelerometer lag bw(rps) = 2pi*10hz

k_tau_x6 = 62.8 % pitch rate gyro lag bw(rps) = 2pi*10hz

%****precomputed constants *******

d11= Izw+(mw+M)*r*r; % precomputed parameter used by nl_pend state function

%**************************************************************************

% Control Law Determination using LQR method

%**************************************************************************

% xd= Ax + Bu , y=Cx + Du

%********* x=[xm,xdm,theta,thetadot,ax,gyro]*************

%x(1)=xm=xw/r+theta, relative rotation,(+ cw looking at rt wheel, unit= rad)

%x(2)=motor rate xdm=xdw/r+thetadot , (+ cw rate looking at rt wheel,rad/s)

%x(3)=tilt angle = theta (+ ccw looking at rt wheel, rad )

%x(4)=tilt angle rate = thetadot (+ ccw looking at rt wheel, rad/s)

%x(5)=lagged fore/aft accelerometer=ax (+ along +x body axis , g's,

% Accelerometer filter bw=k_tau_x5 rad/s

%x(6)=lagged rate gyro{thetadot} (+ ccw looking at rt wheel, rad/s)

% Rate gyro filter bw=k_tau_6 rad/s

%compute A and B state matrices for linear analysis

[A,B] = pend_A_B(g ,M,l,Izb,mw,Izw,r,la,k_tau_x5,k_tau_x6);

C=eye(6) %make observeable to test full state feedback

D=[0 0 0 0 0 0 ]';

sys=ss(A,B,C,D); %create state space model

%now add motor dynamics by closing back emf loop and convert sysm.b to pwm

' *******System with motor dynamics ******************************'

K_motor=[0 tq_per_xmd 0 0 0 0]; % gain assuming negative feedback

sysm.a=A-B*K_motor ;

sysm.b=B*tq_per_pwm;

sysm=ss(sysm.a,sysm.b,C,D)

'******* eigenvalues of motor system u=0 ****************************'

oleig_motor=eig(sysm.a)

sysm1=ss(sysm.a+sysm.b*[0 0 -621 -90 0 0],sysm.b,C,D)

%these function only work in Matlab

%tfm=zpk(tf(sysm1));

%x1num=tfm(1).num

%x3num=tfm(3).num

%x1x3=tf(x1num,x3num)

%sisotool(tfm(2))

31

'check uncontrollable states of continuous system with motor'

co=ctrb(sysm.a,sysm.b);

ucntrstates=length(sysm.a)-rank(co)

%we know that rate x(4) and lagged rate gryo x(6) give an uncontrollable state

%but all we need is x(4) controllable

'check unobservable states of continuous system with motor'

ob=obsv(sysm.a,C);

uobsrvstates=length(sysm.a)-rank(ob)

dsysm=c2d(sysm,dt) ;

AD=dsysm.a

BD=dsysm.b

'******** Set weighting factors for optimal feedback gain '

Q=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0]

R=wu*[1]

'********* now compute optimal gain from Q and R weighting matricides '

[K,S1,E1]=lqr(sysm.a,sysm.b,Q,R); %continuous version

K

%using the discrete version with dt sampling should not be used when including

%any dynamics with eigenvalues over faster than the Nyquist sampling limit

%of .5/dt or ~25hz. Practically, with rectangular integration, we should

%put the limit at 10hz which where our sensor eigenvalues lie.

%Note that the one of the closed loop eigen values is about 256rps or 41 Hz.

%So we should use the continuous system derived gains.

ecl_dlqr=log(ED1)/dt % eigenvalues of closed loop continuous s=ln(z)/dt

factor=1.; %can use to check gain margin

%here we can override the derived gains and fool around to get best

%performance

%K =[0 -12. 433. 60. 0 0]; This works better in the actual robot

%K = [ -0 -22*factor 661*factor 96*factor 0 0];

%dsysmcl=ss(dsysm.a-dsysm.b*K,dsysm.b,dsysm.c,dsysm.d,dt);

%' ****** continuous closed loop system with u=-K*x *******************'

% sysmcl=d2c(dsysmcl);

%ecl_act=log(eig(AD-BD*K))/dt %actual eigenvalues with final gain selection

%ecl_act=eig(sysmcl.a)

ecl_act=E1

' ********* gain used to close loop *******************';

K

Kpsf=K/slope_factor %K per slope_factor is K divided by slope_factor

%The motor command is multiplied by slope_factor later on in simulation.

%********* Ready to simulate **************************************

t=0:dt:T; %set time array for output

32

nlen=max(size(t));

% t=zeros(1,nlen);

x=zeros(6,nlen);

x_est=zeros(6,nlen);

%z=zeros(5,nlen);

u=zeros(1,nlen);

x(3,1)=-5/rad; %****place initial tilt input here and convert to rad*****

x(5,1)=x(3,1); %initialize lagged x(5) = x(3)

x(6,1)=x(4,1); %initialize lagged x(6) = x(4)

x_est(:,1)= x(:,1);

x_true = x(:,1);

x_est_1_last=x_est(1,1);

x_est_3_last=x_est(3,1);

for j=1:nlen-1;

x_enc=3.*x(1,j); % encoder is geared 3 times faster than wheel

xA_D_in=[x_enc ;x_true(6);x_true(5)]; %input to a/d

%xA_D_in= [ wheel encoder, thetadot lag, az lag]

if(sw_A_D_in==0 )xA_D=xA_D_in;

end

x_est(1,j)=(xA_D(1))/3.; %divide by 3 to get motor (xm) rotation

x_est(2,j)=(x_est(1,j)-x_est_1_last)/dt; %derive motor rate (xmd)

x_est(3,j)=x_est_3_last + dt*(xA_D(2)+k_tau*(xA_D(3)-x_est_3_last));

% x_est(3,j)= blended pitch angle =integration of ax blended pitch rate

% with gain k_tau. This makes long term pitch angle = to ax to halt drift

x_est(4,j)= xA_D(2) ; % this is a/d version of x_true(6) lagged gyro

x_est(5,j)= xA_D(3) ; % this is a/d version of x_true(5) lagged accel

x_est(6,j)= xA_D(2) ; % same as x_est(4)..don't really use this in control law

x_est_1_last=x_est(1,j);

x_est_3_last=x_est(3,j);

% 'x est'

% x_est(:,j)

% 'x'

%outx=x(:,j); %just for printout

%*****simulation of control law processing ********

%ucmd=-Kpsf*x(:,j) ; % use this to bypass a/d and sensor processing

ucmd=-Kpsf*x_est(:,j); %use this for normal a/d and sensor processing

%add dead zone compensation function

if ucmd>0 ucmd=ucmd+dz_comp;

elseif ucmd<0 ucmd=ucmd-dz_comp;

end

% limit cmd to servo at ulimit

if ucmd>ulimit ucmdlim=ulimit;

elseif ucmd<-ulimit ucmdlim=-ulimit;

else ucmdlim=ucmd;

end

%compute controller dead zone

if ucmdlim>u_ctrl_dz u_ctrl_lim=u_ctrl_dz;

elseif ucmdlim<-u_ctrl_dz u_ctrl_lim=-u_ctrl_dz;

else u_ctrl_lim=ucmdlim ;

end

33

u(j)=ucmdlim-u_ctrl_lim; %this completes dead zone

for m=1:steps_per_sample;

%compute motor torque input outside of 18.5ms processor loop

u_motor=u(j)-pwm_per_xmd*x_true(2); %add motor back emf term

if(x_true(2)==0)

Cs_pwm=limit(u_motor,-Cs_max_pwm, Cs_max_pwm) ;%compute static friction

else Cs_pwm=0.;

end

u_frict=-Cs_pwm-Cd_pwm*sign(x_true(2))-Cv_pwm*x_true(2);%total friction (pwm units)

u_true=slope_factor*(u_motor + u_frict); %increase cmd to get max rpm with u=pwm_max

xd_true = nl_pend(x_true,u_true*tq_per_pwm,g,M,l,Izb,r,la,k_tau_x5,k_tau_x6,d11);

%xd_true =A1*x_true +B1*u_true ; %linear model for comparison

x_true=xd_true*dt_true+x_true;

end

x(:,j+1)=x_true; % refresh true sample data

%out=x(:,j+1);

if abs(x(3,j))>.7 x(3,j) ,break; %end simulation is we exceed 40 deg tilt

end

end %*******end simulation loop*********

%

close all; %start fresh with figures

figure(1)

plot(t,x(1,:),'g');

title('x1 --motor angle rad');

hold on

[xs,ys]=stairs(t,x_est(1,:));

plot(xs,ys);

hold off

legend( 'x.true' , 'x.est', 4)

figure(2)

plot(t,x(2,:),'g');

title('x2 --motor rate rad/sec');

hold on

stairs(t,x_est(2,:));

hold off

legend( 'x.true' , 'x.est', 4)

figure(3)

plot(t,rad*x(3,:),'g'); %angle in deg

title('x3 --angle deg');

hold on

stairs(t,rad*x_est(3,:));

hold off

legend( 'x.true' , 'x.est(blended ax)', 4)

figure(4)

plot(t,rad*x(4,:),'g'); %angle rate in deg/sec

title('x4 --angle rate deg/s');

hold on

plot(t,rad*x_est(4,:));

hold off

legend( 'x.true' , 'x.est', 4)

figure(5)

plot(t,rad*x(5,:),'g'); %accelerometer deg= g's*rad

34

title('x5 --accel angle(deg) vs --x3 pitch angle(deg) ');

hold on

%stairs(t,rad*x.est(5,:),'r');

plot(t,rad*x(3,:),'r'); %angle in deg

hold off

legend( 'accel true' , 'pitch true', 4)

figure(7)

stairs(t,u(1,:)); %pwm cmd limited and dz

title('u --motor command');

hold on

plot(t,-Kpsf*x(:,:),'g'); %pwm cmd

hold off

legend(' controller output' , ' u=K*x.true',4)

end %end vex_pend function

% nonlinear state rate computation given x_d=funct(x,c,param)

% x={xmd_rps,xm_rad,theta_rad,thetad_rps)

% c=torque input generated from motors

% param are mass, dimensional, and precomputed constants

%d11= Izw+mw*r*r+m*rw*rw is precomputed input

lc = l*cos(x(3)) ;

ls = l*sin(x(3)) ;

d12 = m*r*lc ;

d21 = d12 ;

d22 = Ib + m*l*l ;

det = d11*d22 - d12*d12 ;

e1 = m*r*ls*x(4)*x(4) - c;

e2 = m*g*ls + c;

xd(1) = x(2);

xd(2) = (-e1*(d22+d12) + e2*(d11+d12))/det;

xd(3) = x(4) ;

xd(4) = (-e1*d12 + e2*d11)/det ;

%xacc= -r*cos(?)*( ?_dd - ?m_dd) - la* ?_dd + g*sin(?)

%xacc= -r*( d22*e1 – d12*e2)/det - la*(-d21*e1 + d11*e2)/det+g*sin(?)

xd(5)=(sin(x(3))+(-r*cos(x(3))*(xd(4)-xd(2))-la*xd(4))/g-x(5))*k_tau_x5 ;

xd(6)= (x(4)-x(6))*k_tau_x6;

xd=xd';

end

function [A,B] = pend_A_B(g ,m,l,Izb,mw,Izw,r,la,k_tau_x5,k_tau_x6)

%This function computes the A and B linear state and control matrices for

% x={xmd_rps,xm_rad,theta_rad,thetad_rps, acc_lag, gyro_lag)

% x_d = Ax + Bu

p11 = Izw + (mw +m)*r^2

p12= m*r*l

p21= m*r*l

p22 = Izb + m*l*l

detp = p11*p22 - p12*p21

A=zeros(6) ; B=zeros(1,6)';

B

A(1,2) =1. ;

A(2,3) = m*l*g*(p11+p12)/detp ;

A(3,4) =1.;

A(4,3) = m*l*g*p11/detp ;

A(5,3) =((r*p12-la*p11)*m*l/detp + 1)*k_tau_x5

35

A(5,5) = -k_tau_x5;

A(6,4) = k_tau_x6 ;

A(6,6) = - k_tau_x6;

B(2)=(p21+p22 +p11 + p12)/detp;

B(4)=(p21 + p11)/detp;

B(5)=(r*(p22 + p12)-la*(p21 + p11))/detp/g*k_tau_x5

B(6)= 0;

% another way to compute xacc= [(-r-la)x_d(4) + r*x_d(2))/g + x(3)

%x_d(4)= A(4,3)*x(3) + B(4)*u

%x_d(2)= A(2,3)*x(3) + B(2)*u

%xacc = (((-r-la)(A(4,3)+ r*A(2,3))/g + 1)*x3 + ((-r-la)*B(4) +r*B(2))/g*u

%x_d(5) = (xacc-x(5))*k_tau_x5

%A(5,3) = (((-r-la)*A(4,3)+ r*A(2,3))/g + 1)*k_tau_x5

%B(5) = ((-r-la)*B(4) +r*B(2))/g*k_tau_x5

%These check numerically with above so ok

end

%limit function... requires ul>ll

if(x> ul) out=ul;

elseif x<ll out=ll;

else out= x;

end

end

36

B.1 Sample Matlab output:

This is the response to an initial -5 deg tilt offset.

Output from OCTAVE will be slightly different.

steps_per_sample = 18

sw_A_D_in = 1

N= 10

sf_encoder = 787.8170

sf_az = 300

cnts_per_mv = 0.2048

ozpdyne = 3.5971e-005

l= 6.3500

l_x = 2.5400

l_y = 20.3200

l_z = 20.3200

37

b_vol = 1.0488e+003

rho = 1.1400

M_est = 1.1956e+003

Izb_est = 4.1782e+004

M = 1.1482e+003

Izb = 4.6035e+004

r= 3.4290

wheel_width = 1.7000

mw_est = 143.1029

mw = 113.4000

Izw = 1.3334e+003

ulimit = 127

u_ctrl_dz = 10

38

dz_comp = 0

m_tqmax = 7343648

m_wmax = 10.4720

m_gear = 1

m_num = 2

tq_per_pwm = 115648

tq_per_xmd = 1.4025e+006

pwm_per_xmd = 12.1276

u_frict_dz = 40

u_ratio = 0.7000

cv_ratio = 0.1000

Cd_pwm = 21

Cv_pwm = 0.2865

39

slope_factor = 1.1890

k_tau = 2

k_tau_x5 = 62.8000

k_tau_x6 = 62.8000

p11 = 1.6167e+004

p12 = 2.5001e+004

p21 = 2.5001e+004

p22 = 9.2333e+004

detp = 8.6771e+008

B=

0

0

0

0

0

0

A=

0 1.0000 0 0 0 0

0 0 339.2316 0 0 0

40

0 0 0 1.0000 0 0

0 0 133.2205 0 0 0

0 0 86.3673 0 0 0

0 0 0 0 0 0

B=

1.0e-003 *

0

0.1827

0

0.0474

0.0220

0

C=

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

ans =

a=

x1 x2 x3 x4 x5 x6

x1 0 1 0 0 0 0

x2 0 -256.2 339.2 0 0 0

x3 0 0 0 1 0 0

x4 0 -66.54 133.2 0 0 0

x5 0 -30.82 86.37 0 -62.8 0

41

x6 0 0 0 62.8 0 -62.8

b=

u1

x1 0

x2 21.12

x3 0

x4 5.487

x5 2.541

x6 0

c=

x1 x2 x3 x4 x5 x6

y1 1 0 0 0 0 0

y2 0 1 0 0 0 0

y3 0 0 1 0 0 0

y4 0 0 0 1 0 0

y5 0 0 0 0 1 0

y6 0 0 0 0 0 1

d=

u1

y1 0

y2 0

y3 0

y4 0

y5 0

y6 0

Continuous-time model.

ans =

****************************

oleig_motor =

42

-62.8000

-62.8000

6.8861

-6.5424

-256.5368

a=

x1 x2 x3 x4

x1 0 1 0 0

x2 0 -256.2 -1.278e+004 -1901

x3 0 0 0 1

x4 0 -66.54 -3274 -493.8

x5 0 -30.82 -1492 -228.7

x6 0 0 0 62.8

x5 x6

x1 0 0

x2 0 0

x3 0 0

x4 0 0

x5 -62.8 0

x6 0 -62.8

b=

u1

x1 0

x2 21.12

x3 0

x4 5.487

x5 2.541

x6 0

c=

x1 x2 x3 x4 x5 x6

y1 1 0 0 0 0 0

y2 0 1 0 0 0 0

y3 0 0 1 0 0 0

y4 0 0 0 1 0 0

y5 0 0 0 0 1 0

43

y6 0 0 0 0 0 1

d=

u1

y1 0

y2 0

y3 0

y4 0

y5 0

y6 0

Continuous-time model.

ans =

ucntrstates =

ans =

uobsrvstates =

AD =

Columns 1 through 5

0 0.0050 1.3244 0.0194 0

0 -0.0038 1.0128 0.0186 0

44

0 -0.2598 1.1825 1.0128 0

0 -0.0498 0.5666 0.0070 0.3129

0 -0.1535 0.5452 0.6912 0

Column 6

0

0

0

0

0

0.3129

BD =

0.0012

0.0820

0.0003

0.0214

0.0041

0.0127

ans =

wu =

100

Q=

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 0 0 0

45

0 0 0 0 0 0

0 0 0 0 0 0

R=

100

ans =

K=

Columns 1 through 5

Column 6

ecl_dlqr =

-256.5404

-0.0082

-6.5424

-6.8860

-62.8000

-62.8000

ans =

46

ecl_act =

-62.8000

-62.8000

-256.5455

-6.5424

-6.8861

-0.0082

K=

Columns 1 through 6

Kpsf =

Columns 1 through 6

The green lines are the truth states and blue lines include A/D sampling

effects and represent the states used in the robot control law feedbacks.

47

x2 --motor rate rad/sec

5

4.5

3.5

2.5

1.5

0.5 x.true

x.est

0

0 1 2 3 4 5 6 7 8 9 10

x3 --angle deg

1

-1

-2

-3

-4

x.true

x.est(blended ax)

-5

0 1 2 3 4 5 6 7 8 9 10

48

x4 --angle rate deg/s

20

15

10

-5

x.true

x.est

-10

0 1 2 3 4 5 6 7 8 9 10

u --motor command

90

80

70

60

50

40

30

20

10 controller output

u=K*x.true

0

0 1 2 3 4 5 6 7 8 9 10

49

C. MPLAB vex bal_bot source ‘C’ code

http://www.vexforum.com/local_links.php?action=jump&id=53&catid=26

D. Tutorial Links

http://www.engin.umich.edu/group/ctm/examples/pend/invpen.html

Undergraduate Lecture notes: LQG/LQR Controller tutorial

http://www.ece.ucsb.edu/~hespanha/ece147c/web/lqrlqgnotes.pdf

Wikipedia LQR

http://en.wikipedia.org/wiki/Linear-quadratic_regulator

John Hopkins Signals Systems Controls Demos

http://www.jhu.edu/~signals/

Linear time Invariant systems and Convolution

http://www.jhu.edu/~signals/lecture1/frames.html

http://www.jhu.edu/~signals/explore/index.html

http://hyperphysics.phy-astr.gsu.edu/hbase/hframe.html

50

E Moment of Inertia Calculations

http://hyperphysics.phy-astr.gsu.edu/hbase/inecon.html

BODY:

Lx Lz

Ly

Lycg

z axis of

rotation

The density rho=M/V. Lycg is the distance of the axis from the center of

gravity along the y axis.

the component distances from the axis of rotation.

51

If we take the axis to be through the cg, ie Lycg=0 the moment about the cg

Iz_cg we get

astr.gsu.edu/hbase/parax.html#pax that states that if you move the axis but

keep it parallel to the cg axis then the resulting moment of inertia Iz_new is

the Iz_cg plus the mass times the square of the distance moved . I.e.

between the two axes.

So if the wheel axle axis is a distance Lycg from the cg in the y direction

then the moment about the wheel axle is

Or if we know Iz_axle,

If we turn the pendulum robot upside down and support it by the wheel axles ( wheels off)

such that it can freely rotate about the axle it becomes a pendulum which if we measure

the period of oscillation after being disturbed, can indirectly give us Iz_cg from the above

equation. The axle can sit on and rock on a sharp edge or if disconnected from the motor

the axles can be held securely and the robot is free to rotate on axle bearings. After

determining the period, we turn the robot on its large face and balance it on a knife edge

parallel to the z axis. We then measure the distance from the balance point to the wheel

axis and this gives us Lycg. We then weight the robot and determine its mass M=

Weight/g.

52

For small deflections the formula for Iz_wheel is

Iz_axle_measured = g*Lycg*M*(period/(2*π))2

using the parallel axis theorem to shift results to cg gives

53

WHEEL:

z axis

of

Iz_wheel_cg = r*r*M_w/2 .

Often times the wheel has more mass distributed toward the outside of the

rim so this formula under predicts the actual moment of inertia. We can use

the same technique to measure Iz_wheel offset by putting a pin through the

tire or fastening a thin axle to the top of the tire and making a pendulum, the

period of which can give us Iz_wheel_offset as was done for the body.

Suppose the offset from the wheel cg is equal to r_offset… then

54

F. Steady-state motor rate vs pwm_cmd data

With the vex motors installed in the Bbot, steady state pwm commands were

ranging from 0 to 255 and back . The steady state speed as measured by the

encoders was obtained using the IFI debug window and is plotted in Figure

F1. The behavior is clearly nonlinear, reasonably symmetric and due to

noisy encoder outputs at the higher speeds it is difficult to judge the actual

maximum speeds. These motors are specified at 100 rpm or 10.5 rps. The

black dashed line is an approximation used to establish w_max and

pwm_max for these motors. The dead zone is also noted to be around 30

total width. The slope ,w_per_pwm is 1/3.

15

w__max=11rps

at pwm__max=48

10

-5

-10

-15 0

-150 -100 -50 50 100 150

55

As a secondary check of the controller response, an old (Futaba??) servo was taken apart

to see what the voltage on the motor was as a function of processor pwm output. The

feedback resistor was removed from the servo to allow it to operate as a motor. Also, the

gears were removed so the motor was free running with the minimum load.

A charged battery was used and the controller was able to deliver 7.7v to the motor. A

digital Fluke voltmeter was used to measure the voltage across the motor. There does not

seem to more than one count of dead zone on this servo and the slope is about 7.7v per 28

cnts. So outside the dz, it takes about 28 pwm cnts to get max rate by our straight line

approximation. This checks fairly well with what we saw in Fig F1.

(non vex servo motor without feedback

operating with no-load)

10

8

6

volts across motor

4

volts

2

0

0

-125 -100 -75 -50 -25 25 50 75 100 125

-2

Figure F2 vex pwm output 256 cnts per 1ms

56

Appendix G

Motor Model :Dynamic_dz and Static_dz Determination

With the motors installed in the robot, the vex processor was used to

gradually increase pwm_cmd to the motor controller in the forward

direction. The motors initially stay stopped until the pwm_cmd

increases enough to create a torque that overcomes static friction

torques. The point at which this happens is called static_dz_fwd

The pwm_cmd is then decreased slowly until the motors stop. The

pwm_cmd at this point is called dynamic_dz_fwd. We repeat the

process in the other direction to measure static_dz_aft and

dynamic_dz_aft.

Test results: aft rotation fwd rotation

static_dz/ dynamic_dz dynamic_dz/ static_dz

rt motor : -12/-10 18/20

lt motor : -19/-13 18/22

lt : dynamic_dz= (dynamic_dz_fwd -dynamic_dz_aft )/2 = 15.5

avg = 14.75

lt: static_dz = ( static_dz_fwd – static_dz_aft)/2 = 20.5

avg= 18.25

The lt motor has an offset bias to the dead zone = +5 dynamic

+3 static

These offsets can be used in the target control laws to balance the

motor inputs. The rt motor in this case has more friction than the lt.

57

What values we pick for the model determines how conservative we

want to be ….we can use minimum values, avg or max.

For the simulation , the following values are used:

58

Change Log:

Rev1

1.1 Corrected d21 and d22 of Nonlinear Equations in Section 4.

Secondary Effects. No difference in linearized equations.

59

- A New Configuration of Two-Wheeled Inverted Pendulum: A Lagrangian-Based Mathematical ApproachUploaded byCyberJournals Multidisciplinary
- Self Balancing With Pid ControlUploaded byengrumar
- Two wheeled self balancing robot with fuzzy controllerUploaded byAbu Anas Shuvom
- Thesis - Self Balancing RobotUploaded byEcho Alex
- Inverted pendulumUploaded byArslan Tahir
- Control of Inverted PendulumUploaded byMohammad Umar Rehman
- Self Balancing RobotUploaded byapurvabhardwaj
- inverted_pendulumUploaded byNassim Doukhi
- Inverted Pendulum Supporting ControlsUploaded byThanhLong
- Matlab_Accelerating Flight Vehicle DesignUploaded byuwmieee
- Modeling an Inverted PendulumUploaded byjunaid_honey8389677
- Wheeled Inverted Pendulum EquationsUploaded byMunzir Zafar
- Linear Quadratic Regulator (LQR) Controller Design for DC Motor Speed Using Matlab ApplicationUploaded byVinicius
- Building a Two Wheeled Balancing RobotUploaded byCp Em Pheerador
- Inverted PendulumUploaded byLingxi Huang
- Inverted PendulumUploaded byDheeraj Gaddi
- Self-balancing robotUploaded byHoang Minh Chung
- Self balancing RobotUploaded byMuhammadHaris
- InvertedPendulumUploaded byzaccaro_86
- Inverted PendulumUploaded byTarun Kothari
- Control of Two-wheels Inverted Pendulum Mobile Robot Using Full Order Sliding Mode ControlUploaded byc0d3r
- Inverted Pendulum MATLAB ManualUploaded bynutanaya
- 2003 Balance OoiUploaded byKevin Hoang Soy Yo
- Self Balancing Two Wheeled Robot ReportUploaded bynetlvr0
- Elektor Wheelie Control LQ ConstrolUploaded byKujtim_Iljazi
- Inverted Pendulum NotesUploaded bystephanie-tabe-1593
- Rotary Inverted PendulumUploaded byAmine Y. Alami
- Self Balancing RobotUploaded byS Bera
- LQR for Rotating inverted pendulumUploaded byValery Gaulin

- Scilab CodeUploaded bytarasasanka
- The Playful Language of MathUploaded byGANAPATHY.S
- Solidification of Metals 8Uploaded byThaiminh Vo
- Rak-54 1300 Hyva Lukea Timoshenko Beam TheoryUploaded byCristina Vlaicu
- Vehicular Cabins’ Thermal Comfort Zones; Fanger and Berkley ModelingUploaded bySEP-Publisher
- TestUploaded byNandkumar Chinai
- inao-srUploaded byArafat Khan
- Natures's Own Sunlight Proves Einstein WrongUploaded bySavvy S
- Pumps in Series and ParallelUploaded bySuhadahafiza Shafiee
- ENG_DS_MHR_A1Uploaded byJuan Olguin
- Idolikecfd Vol1 2ed v2p2Uploaded bymynameshu
- Anna University I year Engineering 2017 Regulation - Lecture Notes, Study Materials and Important questions answersUploaded byBrainKart Com
- IJETR021851Uploaded byerpublication
- CHAPTER 4Uploaded byI K Wirawan
- 04 Essence of Three-phase FriedliUploaded byAnonymous NGXdt2Bx
- What is Adsorption IsothermUploaded bysimha_86
- Mass Transfer in a Short Wetted-Wall Column.pdfUploaded byJay Maradiya
- Photometric Law of DistanceUploaded byJose Galvan
- PDE Lecture 1Uploaded byVishnu
- stp_booksUploaded byAnonymous jxpmO7pOt
- Shawyer EM Drive TheoryUploaded byPabloSilveira
- Earthquake Design of Rectangular UndergroundUploaded bytrabajosic
- Thin Film ReactorsUploaded byThanh Huong
- 2012 CLEO Abstracts WednesdayUploaded byFabrizio Consoli
- Impact of JetUploaded byRoshane Nanayakkara
- Torsional Mesh Stiffness of Spur GearsUploaded bylukehunter
- Birendra kumar pptUploaded byPriyanshu Kumar
- Improvement of Dynamic Voltage Stability in a Microgrid by Voltage StabilizerUploaded byIJMER
- 2Laminar SimpleUploaded byAulia Rahman
- Solution-Manual-for-Vector-Mechanics-for.pdfUploaded byYoshi Leong