White PaperRev 1 , 8/20/07

Modeling a Two Wheeled Inverted Pendulum Robot(Draft)

Author : VamfunVamfun@yahoo.com

1

Table of Contents

1. Introduction 2. Free Body Definitions3. Force and Moment Equations4. Nonlinear Model5. Linear Model6. Sensor Model7. Motor Model8. Motor Controller Dead Zone9. 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

VamfunRetired Lockheed Avionic and Control Systems EngineerMentor Robodox Team 599

4

2. Free Body Definitions

U

U

N

N

Ө

C

C

m*g

FxgFyg

X axis

Y axis

Z axis

Y axis

Right side viewRear view

xcg, ycg

xw,yw

FREE BODY VARIABLE DESCRIPTIONS

m=mass of bodyg=gravitymw=mass of wheelr=radius of wheell=distance from wheel axle to body cg along body y axisxw,yw position of wheel axle centroidxcg, ycg position of body cgFxg, Fyg ground forcesN,U forces on axleC torque applied to axle by motorӨ=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 axisIzw=principle moment of inertia about cg wheel z axis

l

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 axes2) m* ycg_dd = N – m*g :inertial axes3) 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 axes5) mw*yw_dd = Fyg – N – mw*g :inertial axes6) Izw* Өw_dd = r*Fxg – C :body axes

Kinematic linkage relationships

7) xw = - r*Өw :assuming no slippage of wheel8) xw_d = - r*Өw_d9) 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 = e1d21* Ө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)/det20) Ө_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)/det23) Ө_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 ModelThe 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 0sin Ө*sinӨ= δӨ* δӨ = 0

9

From eq 22) and 23)

26) x1_dd = ((-p21- p22)*e1 + (p11+p12)*e2)/detp27) 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 *ux3_d = x4x4_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 Ba12 =1. a23 = m*l*g*(p11+p12)/detp b2 = (p21+p22 +p11 + p12)/detpa34 =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_5B(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_x6B(6) = 0

13

7. Motor ModelFigure 1 shows a simplified block diagram of the 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

Motor Controller

Dead Zone Gain Motor gear

Body/Wheel

Model

Sensors

Vex Microprocessor

a/d Gain d/a

Fig 1 Block Diagram of Inverted Pendulum Control System

Cpwm

Left+Right

X

14

.

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)

pwm_cmd

Dead zone width +_dz

pwm_dz

Gain tq_per_pwm

Өm_d

Net Motor Cm torque

motor speedw

Vex Processor

Body/Wheel Dynamics

Gain m_gear

Gain tq_per_w

FrictionFunctionsCs,Cd,Cv

C Toeal Torque

Cf friction torque

Body/Wheel Dynamics

Fig 2 Motor/controller analysis model

+

++

--

motorback EMFtorque

+-ctrl_pwm_lim

pwm_dz_lim

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_maxnote: 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_limelseif (x<lower_lim) out=lower_limelse out=xend

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/Friction/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 zeroSet Tmax and T % Tmax =maximum simulation time, T= sample interval While (t<=Tmax) % Iterate problem until max time is reachedu=f(t ) % compute input as function of timex_d=A*x+B*u % compute state rate x_d(t)x = x + x_d*T % update statet = t + T % update timeEnd 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 constantsQ=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=2888and 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 functiona=[2 , 1 ; 3 ,5]b=[1  0 ; .5  3]  .a*bb*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*aYour 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: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/07function InvertPend_vex_6state_nlsw_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 parametersfor i=1:n_param'******** new parameter ***************** ',vex_pendulum_main(param(i)) %call the main function end %end forelse vex_pendulum_main('none');end %end if sw_paramend %end InvertPend_vex_6stat_nl function vex_pendulum_main(param_value) % main functionla=2.54 ;% put default parameter value hereif ~strcmp(param_value ,'none')la = param_value % Here 'la' is the parametric variable as exampleend' ***************************************'% ******* 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.5mssteps_per_sample =fix(dt/.001) % round to nearest integer relative to .001sdt_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 effectsN=10 % 10 bit a/d%1024 cnts with 10 bit a/d

29

sf_encoder=4950/2/pi() %mv/rad =4950mv/rev*rev/6.28radsf_az=300 %mv/gsf_thetadot=2*180/pi(); %mv/rps =2mv/deg_per_s*deg_per_radcnts_per_mv=2^N/5000 %cnts/mvsf=cnts_per_mv*[sf_encoder sf_thetadot sf_az] %cnts/unit % ****conversion constants *********rad=180/pi(); %deg/radozpdyne=(1/0.278)*1.0e-5 %oz/N*N/dynecmpin=2.54; %cm per in conversiongramspoz=28.35; % grams per oz conversion%**** mass properties *****************%pendulum dim l_x,l_y,l_z (width,height,depth) z is parallel to wheel axisg = 980.7; % cm/s**2l = 2.5*cmpin %cm= in*cmpin , this is the distance to cg of body from axlel_x=1*cmpin %width of body normal to pivotl_y=8*cmpin %height of bodyl_z=8*cmpin % cm=in*cmpin width of beam parallel to pivotb_vol=l_x*l_y*l_z% rho=1.14 % densityM_est=rho*b_vol % estimated mass assuming uniform densityIzb_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*MPeriod=.714;Izb=g*l*M*(Period/2/pi())^2-l*l*M%Wheel radius =1.35inr=1.35*cmpin %radius in cmwheel_width= 1.7 %cmmw_est=rho*3.14*r*r*wheel_width*2 % theory %measured mass of small wheel=2 ozmw=(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 rpmpwm_max=127; %Maximum motor commandulimit=pwm_max %motor pwm cmd limitu_ctrl_dz=10 %1/2 of total controller pwm deadzone nominal=5 pwm cntsdz_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*cmm_wmax=100*2*pi()/60. %motor no-load speed rpm*2pi/60sec/min=rad/secm_gear=1 %torque increase due to gearing (motor rev/wheel rev)m_num=2 %number of motorstq_per_pwm=m_tqmax/pwm_max*m_gear*m_num %gain = torque/pwmcmdtq_per_xmd=m_tqmax/m_wmax*m_gear*m_gear*m_num %gain = torque/xmdpwm_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_dzCs_max_pwm=u_frict_dz-u_ctrl_dz; %maximum static frictionu_ratio=.7 % ratio between dynamic and static friction coefficientscv_ratio=.1 % ratio of Cv@max rpm/Cs_max Cd_pwm= u_ratio*Cs_max_pwm % Max dynamic friction torqueCv_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 %cmk_tau=2 % 1/tau_wo for accelerometer blending with pitch anglek_tau_x5 = 62.8 % accelerometer lag bw(rps) = 2pi*10hzk_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 feedbackD=[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 *******************';KKpsf=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 wheelxA_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 functionif(sw_A_D_in==0 )xA_D=xA_D_in; endx_est(1,j)=(xA_D(1))/3.; %divide by 3 to get motor (xm) rotationx_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 driftx_est(4,j)= xA_D(2) ; % this is a/d version of x_true(6) lagged gyrox_est(5,j)= xA_D(3) ; % this is a/d version of x_true(5) lagged accelx_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 processingucmd=-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 ulimitif ucmd>ulimit ucmdlim=ulimit; elseif ucmd<-ulimit ucmdlim=-ulimit; else ucmdlim=ucmd; end%compute controller dead zoneif 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 loopu_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 frictionelse Cs_pwm=0.;endu_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_maxxd_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 comparisonx_true=xd_true*dt_true+x_true; endx(:,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 endend %*******end simulation loop*********% close all; %start fresh with figuresfigure(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 degtitle('x3 --angle deg');hold onstairs(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/sectitle('x4 --angle rate deg/s');hold onplot(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 deghold off legend( 'accel true' , 'pitch true', 4)figure(7)stairs(t,u(1,:)); %pwm cmd limited and dztitle('u --motor command');hold onplot(t,-Kpsf*x(:,:),'g'); %pwm cmdhold offlegend(' 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*lp22 = Izb + m*l*ldetp = p11*p22 - p12*p21 A=zeros(6) ; B=zeros(1,6)'; BA(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 okend function out= limit(x,ll,ul)%limit function... requires ul>llif(x> ul) out=ul;elseif x<ll out=ll;else out= x;endend

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 =

0

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 =

1

ans =

check unobservable states of continuous system with motor

uobsrvstates =

0

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

0

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

0 1 2 3 4 5 6 7 8 9 10-5

-4

-3

-2

-1

0

1x3 --angle deg

x.true

x.est(blended ax)

0 1 2 3 4 5 6 7 8 9 100

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5x2 --motor rate rad/sec

x.true

x.est

48

0 1 2 3 4 5 6 7 8 9 10-10

-5

0

5

10

15

20x4 --angle rate deg/s

x.true

x.est

0 1 2 3 4 5 6 7 8 9 100

10

20

30

40

50

60

70

80

90u --motor command

controller output

u=K*x.true

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 Demoshttp://www.jhu.edu/~signals/

Most relevant ones:Linear time Invariant systems and Convolutionhttp://www.jhu.edu/~signals/lecture1/frames.html

Exploring the s plane Java applethttp://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:

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.

Lx Lz

Ly

z axis of rotation

Lycg

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:

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

r

z axis of

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.

Figure F1 Steady state motor speed vs pwm_cmd

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

-10

-5

0

5

10

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

straight line approximation

Dead Zone +-15

w__max=11rpsat pwm__max=48

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)

-2

0

2

4

6

8

10

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

Figure F2 vex pwm output 256 cnts per 1ms vo

lts

acr

os

s m

oto

r

volts

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_fwdThe 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_dzrt motor : -12/-10 18/20lt 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 = 14lt : 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