White Paper

Rev 1 , 8/20/07

Modeling a Two Wheeled Inverted Pendulum Robot

(Draft)

Author : Vamfun
[email protected]

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

I decided to build a Vex inverted pendulum robot to use as a teaching vehicle

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

and is featured on these youtube.com links:

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

FREE BODY VARIABLE DESCRIPTIONS

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

mw*g =gravity force on wheel cg

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

4) mw*xw_dd = Fxg – U :inertial axes

5) mw*yw_dd = Fyg – N – mw*g :inertial axes
6) Izw* Өw_dd = r*Fxg – C :body axes

Kinematic linkage relationships

7) xw = - r*Өw :assuming no slippage of wheel

8) xw_d = - r*Өw_d
9) xw_dd = - r*Өw_dd

10) xcg = xw – l*sinӨ

11) ycg = yw + l*cosӨ

6
 12) xcg_d = xw_d – l*cosӨ*Ө_d
13) ycg_d = yw_d – l*sinӨ*Ө_d

14) xcg_dd = xw_dd + l* sinӨ*Ө_d* Ө_d – l*cosӨ*Ө_dd

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:

Өw_dd = g1( Өw , Өw_d , Ө , Ө_d , C)

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

The algebraic approach is to use 9, 14, 15 and 18 to eliminate xw_dd ,

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.

After lots of manipulations, we get the following set of equations:

d11* Өw_dd + d12* Ө_dd = e1

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

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

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

Now solving for Өw_dd and Ө_dd gives

19) Өw_dd = ( d22*e1 – d12*e2)/det

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

where det=d11*d22-d12*d21

Change state variable:

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.

Finally , after substitution

********************************************************
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

24) x_d = A*x +B*u where the state vector is defined as

25) x =[ δӨm , δӨm_d ,δ Ө , δӨ_d ]T

=[ 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)

26) x1_dd = ((-p21- p22)*e1 + (p11+p12)*e2)/detp

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

where p11,p12,p21,p22 ,q1,q2 are obtained by linearizing

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

This process results in:

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

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

substituting x1_dd = x2_d , x3_dd=x4_d and expanding gives the equations

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

TaDa….we are done. We precompute A and B with constant pij’s .

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 :

x5 = lagged accelerometer with 3db bandwidth at k_tau_x5

x6 = lagged pitch rate with 3db bandwidth at k_tau_x6

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

x_d = (x in- x)*k_tau note: k_tau has units of rad/sec

Accelerometer :

28) x5_d = (xacc – x5)*k_tau_x5

Rate gyro :

29) x6_d= (x4 –x6)*k_tau_x6

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

xacc = x_dd_la + g*sin(Ө)

where

x_dd_la = xw_dd*cos(Ө) + yw_dd*sin(Ө) - la* Ө_dd

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.

Using yw_dd = 0 , xw_dd= - r Өw_dd and eq 19) and 20) we get

30) xacc= -r*cos(Ө)*( Ө_dd - Өm_dd) - la* Ө_dd + g*sin(Ө) or

31) xacc= -r*cos(Ө)*( d22*e1 – d12*e2)/det - la*(-d21*e1 + d11*e2)/det +

g*sin(Ө)

Linear Sensor Models

12
Only the accelerometer model needs linearization. Substituting the linear
equivalents into eq 31) gives

δxacc=( -r*(p22*q1 – p12*q2) –la*(-p21*q1 + p11*q2) )/detp + g*x3

recall that q1 = - u and q2 = m*l*g* x3 +u so

32) δxacc=(( r*p12 –la*p11)* m*l*g /detp + g)*x3 + ( r*(p22 + p12) –

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

Expressing eq 28) and 29) in x_d=A*x +B*u form with x augmented to

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

Eq 28) x5_d = (xacc – x5)*k_tau_x5 adds

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

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

Eq 29) x6_d= (x4 –x6)*k_tau_x6 adds

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

Fig 1 Block Diagram of Inverted Pendulum

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

pwm_cmd pwm_dz pwm_dz_lim Gain

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

Fig 2 Motor/controller analysis model

The Vex motors can be modeled simply if we ignore the motor inductance
and just include the back emf terms.

Define :

pwm_cmd= Pulse Width Modulation command from vex microprocessor

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

pwm_cmd_max= 127

m_gear= torque increase due to gearing (=motor speed/ Өw_d)

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

pwm_w_max= pwm command required to get no-load max speed w_max

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

pwm_dz= output of controller dead zone.

w=Өw_d*m_gear = motor speed

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

w_max = no-load maximum motor speed (100rpm for Vex motor)

note: See appendix F for lab test data.

w_per_pwm = slope of speed curve from steady state speed vs pwm

= w_max/(pwm_w_max- dynamic_dz)
note: dynamic_dz is defined in section 8 on the Motor Controller Dead
Zone

tq_per_pwm= gain between motor pwm input and torque output .

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

tq_per_w = back emf torque caused by change in w

= tq_per_pwm/w_per_pwm

tq_per_Өm_d = tq_per_w*m_gear

ctrl_pwm_lim= the value of pwm_dz that gives maximum motor speed.

=pwm_w_max – dz

pwm_dz_lim= the limited value of pwm_dz output.

= 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

The motor torque equation then becomes:

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

Cm = (tq_per_pwm*pwm_dz_lim- tq_per_Өm_d * Өm_d)

8. Motor Controller Dead Zone

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.

pwm_dz= pwm_cmd – limit(pwm_cmd, -dz, dz) where limit is a function :

function out=limit(x, lower_lim, upper_lim)

if (x>upper_lim) out=upper_lim
elseif (x<lower_lim) out=lower_lim
else out=x
end

We can also define two additional dead zone widths:

dynamic_dz and static_dz .

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.

cntrl_dz=10 (assumed from specification data given on vex site)

dynamic_dz=15 (measured)
static_dz= 18 (measured)

9. Motor Friction Model

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 .

Cf= -Cs*(v=0?) - Cd*sign(v) – Cv*v

where (v=0?) =1 if v=0

=0 otherwise.

Cs= limit(Cm, -Cs_max, Cs_max)

18
 { 1 if v>0 }
sign(v)={ 0 if v=0 }
{ -1 if v<0 }

This term is now added to Cm to get total C.

C = Cm + Cf

The dynamic friction Cd is proportional to the normal force between sliding

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.

Similarly, we can define Cv from (dynamic_dz-dz) as

Cd=u_ratio* Cs_max where u_ratio = (dynamic_dz-dz)/(static_dz-dz)

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.

There is no data to rely on for Cv , so it is assumed to be cv_ratio*Cs_max at

w=w_max. Therefore ,

Cv=cv_ratio*Cs_max/w_max.

The nominal value of cv_ratio must be determined experimentally and for

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

Substituting and rearranging we get the difference equation

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.

Compute A and B matrices from mass properties and geometry.

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:

We have the state equations from previous sections:

x_d = A*x + B*u where

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.

The sensor output vector y is given by

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

In state space notation, y is defined by C and D matrices. Note, this C is a

matrix, not the torque input.

y= C*x + D*u where C=3x6 matrix and D =3x1 matrix.

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 = ss(A,B,C,D) where ss is a matlab state space definition function.

‘sys’ is the name of the state space. We can define and access data from sys
by using a membership command i.e.

sys.a =A , sys.b= B etc

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

We can express this in matrix notation as

u = -Km*x + tq_per_pwm*pwm_cmd

where Km=[0 , tq_per_ Өm_d , 0, 0 ,0, 0] .

Substituting for u in our state equation gives

x_d = A*x + B*(-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,

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

where sysm.a = (A-B*Km) and sysm.b =B* tq_per_pwm

The stability of ‘sysm’ can be examined by computing the open loop

(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.

In the appendix B.1 output example

oleig=[ 0 -62.80 -62.80 6.95 -6.50 -206.91]

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.

Finding K with LQR

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.

Q is a 6x6 positive definite matrix and R is a 1x1 positive definite matrix.

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

xTQx + uTRu = x1*x1*q11 + x2*x2*q22 +….+ x6*x6*q66 + u*u*r11.

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 ]

Now we simply use the matlab function ‘lqr’ to find K.

[K, S1, cleig_motor] = lqr(sysm)

In addition to the state-feedback gain K, lqr returns the solution S1 of the

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

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

In the B1 appendix example,

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

K= [ -0.1000 -5.4888 153.6721 22.1151 0 0]

and closed loop stable eigenvalues

cleig_motor = [ -62.8000 -62.8000 -207.0471 -6.5042 -6.9500 -0.0366 ]

25
APPENDICES

A. OCTAVE Program Installation Notes

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:

## System-wide startup file for OCTAVE.

##
## 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");

Note: the addpath() function seems to like forward slashes /.

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”

%Vex Two Wheeled Inverted Pendulum Analysis and Simulation Program

%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:[email protected]
%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

function vex_pendulum_main(param_value) % main function

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

% ****conversion constants *********

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.

% ****** measured mass properties

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

%Motor friction constants

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

%******* sensor data ********************

%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 ]';

'system without motor loop';

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)

%convert to discrete system

dsysm=c2d(sysm,dt) ;
AD=dsysm.a
BD=dsysm.b
'******** Set weighting factors for optimal feedback gain '

wu=1e2 %weight of control input... also the relative weight if q=1

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.

[KD,SD1,ED1]=dlqr(dsysm.a,dsysm.b,Q,R); %discrete version

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];

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

%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);

' *****closed loop eigenvalues *********'

%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;

%*****simulation of sensor processing x_est=function(x)

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]

xA_D=diag(sf)\fix(diag(sf)*xA_D_in); %output of a/d function

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

%update true simulation states

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

function [xd] = nl_pend(x,c ,g ,m,l,Ib,r,la,k_tau_x5,k_tau_x6,d11)

% 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

function out= limit(x,ll,ul)

%limit function... requires ul>ll
if(x> ul) out=ul;
elseif x<ll out=ll;
else out= x;
end
end

Program ends here:

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

sf = 161.3449 23.4684 61.4400

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 =

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

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 =

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

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

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 =

check uncontrollable states of continuous system with motor

ucntrstates =

ans =

check unobservable states of continuous system with motor

uobsrvstates =

AD =

Columns 1 through 5

1.0000 0.0038 0.0194 0.0002 0

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 =

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

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 =

********* now compute optimal gain from Q and R weighting matricies

K=

Columns 1 through 5

-0.1000 -24.2858 661.1562 96.0168 0

Column 6

ecl_dlqr =

-256.5404
-0.0082
-6.5424
-6.8860
-62.8000
-62.8000

ans =

*****closed loop eigenvalues *********

46
ecl_act =

-62.8000
-62.8000
-256.5455
-6.5424
-6.8861
-0.0082

K=

Columns 1 through 6

-0.1000 -24.2858 661.1562 96.0168 0 0

Kpsf =

Columns 1 through 6

-0.0841 -20.4258 556.0718 80.7559 0 0

B.2 Sample Figures

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

Download bal_bot_8.13.07.zip from Vex forum:

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

D. Tutorial Links

Matlab Control Tutorial

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/

Most relevant ones:

Linear time Invariant systems and Convolution
http://www.jhu.edu/~signals/lecture1/frames.html

Exploring the s plane Java applet

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

Mechanics Tutorial from Hyperphysics

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

50
E Moment of Inertia Calculations

Before proceeding, review the hyperphysics Moment topics at

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

BODY:

Lx Lz

Ly
Lycg

z axis of
rotation

Calculating the Principle Moment of Inertia Iz about z axis:

Assume a rectangular solid with uniform mass M and volume V=Lx*Ly*Lz .

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

Iz = rho*Lz*Intgrl((x*x +y*y )*dx*dy ) where x and y are

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

Iz_cg = M/12*(Lx*Lx +Ly*Ly)

There is a parallel axis theorem http://hyperphysics.phy-

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.

Iz_new= Iz_cg + d*d*M. where d is the shortest distance

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

Iz_axle= Iz_cg + Lycg*Lycg*M

Or if we know Iz_axle,

Iz_cg = Iz_axle- Lycg*Lycg*M.

How to Experimentally Determine 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

where g = acceleration of gravity and period is measured in seconds. Finally

using the parallel axis theorem to shift results to cg gives

Iz_cg = Iz_axle_measured – Lycg*Lycg*M .

53
WHEEL:

z axis
of

Wheel of with radius r , width w , uniform mass M_w has a Iz_wheel_cg 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

Iz_wheel_cg= Iz_wheel_offset – r_offset*r_offset*M_w

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.

motor #3and #4 speeds (rps) vs pwm cmd 0->255,255->0

15

w__max=11rps
at pwm__max=48
10

Dead Zone +-15

-5

-10

straight line approximation

-15 0
-150 -100 -50 50 100 150

Figure F1 Steady state motor speed vs pwm_cmd

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.

controller output volts vs pwm

(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

We now compute the total dead zone and divide by 2

rt :dynamic_dz= (dynamic_dz_fwd -dynamic_dz_aft )/2 = 14

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

rt: static_dz = ( static_dz_fwd – static_dz_aft )/2 = 16

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

The rt motor has an offset bias to the dead zone = +8

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:

dynamic_dz=15 and static_dz=18

58
Change Log:
Rev1
1.1 Corrected d21 and d22 of Nonlinear Equations in Section 4.
Secondary Effects. No difference in linearized equations.

59