Scribd Upload a Document Search Documents Explore DocumentsBooks - FictionBooks - Non-fictionHealth & MedicineBrochures/CatalogsGovernment DocsHow-To Guides/ManualsMagazines/NewspapersRecipes/MenusSchool Work+ all

categoriesFeaturedRecentPeopleAuthorsStudentsResearchersPublishersGovernment & NonprofitsBusinessesMusiciansArtists & DesignersTeachers+ all categoriesMost FollowedPopular..Sign Up|Log In..1First Page Previous Page Next Page / 59Sections not available Zoom Out Zoom In Fullscreen Exit FullscreenSelect View Mode View ModeSlideshowScroll ...Readcast Add a Comment Embed & Share Reading should be social! Post a message on your social networks to let others know what you're reading. Select the sites below and start sharing.Readcast this Document.. Login to Add a Comment.. Share & Embed.Add to Collections Download this Document for FreeAuto-hide: on White Paper Rev 1 , 8/20/07 Modeling a Two Wheeled Inverted Pendulum Robot (Draft) Author : Vamfun Vamfun@yahoo.c om 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 U U N N CC m*g Fxg Fyg X axis Y axis Z axis Y axis Right side view Rear view xcg, ycg xw,yw FREE BODY VARIABLE DESCRIPTIONS m=mass of body g=gravity mw=mass of wheel r=radius of wheel l=distance from wheel axle to body cg along body y axis xw,yw position of wheel axle centroid xcg, ycg position of body cg Fxg, Fyg ground forces N,U forces on axle C 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 axis Izw=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 Newtons 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 numbers0 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 pijs . 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. 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 C pwm 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 speed w Vex Processor Body/Wheel Dynamics Gain m_gear Gain tq_per_w Friction Functions Cs,Cd,Cv CToeal Torque Cf friction torque Body/Wheel Dynamics Fig 2 Motor/controller analysis model ++ +-- motor back EMF torque +-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_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 lets 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= [10 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 qs 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 wont 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 didnt 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:Vamfun@yahoo.com %Rev 8/20/07 %For a detailed description of the models see the white paper reference: % "Modeling a Two Wheeled Inverted Pendulum Robot" by Vamfun, Rev 1 8/20/07 functionInvertPend_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 fori=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 functionvex_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=[000000 ]'; '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_xmd0 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=[10 0 0 0 0; 0 10 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); forj=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; elseucmdlim=ucmd; end %compute controller dead zone ifucmdlim>u_ctrl_dz u_ctrl_lim=u_ctrl_dz; elseifucmdlim<-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 form=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 elseCs_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********* %closeall; %start fresh with figures figure(1) plot(t,x(1,:),'g'); title('x1 --motor angle rad'); holdon [xs,ys]=stairs(t,x_est(1,:)); plot(xs,ys); holdoff legend('x.true' ,'x.est', 4) figure(2) plot(t,x(2,:),'g'); title('x2 --motor rate rad/sec'); holdon stairs(t,x_est(2,:)); holdoff

legend('x.true' ,'x.est', 4) figure(3) plot(t,rad*x(3,:),'g'); %angle in deg title('x3 --angle deg'); holdon stairs(t,rad*x_est(3,:)); holdoff 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'); holdon plot(t,rad*x_est(4,:)); holdoff 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) '); holdon %stairs(t,rad*x.est(5,:),'r'); plot(t,rad*x(3,:),'r'); %angle in deg holdoff legend( 'accel true' , 'pitch true', 4) figure(7) stairs(t,u(1,:)); %pwm cmd limited and dz title('u --motor command'); holdon plot(t,-Kpsf*x(:,:),'g'); %pwm cmd holdoff 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)'; 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 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 =000000 A= 0 1.0000 0 0 0 0 0 0339.2316 0 0 040 0 0 0 1.0000 0 0 0 0133.2205 0 0 0 0 086.3673 0 0 0 0 0 0 0 0 0 B = 1.0e-003 * 0 0.1827 0 0.0474 0.0220 0 C =10 0 0 0 0 01 0 0 0 0 0 01 0 0 0 0 0 01 0 0 0 0 0 01 0 0 0 0 0 01 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 10 0 0 0 0 y2 0 10 0 0 0 y3 0 0 1 0 0 0 y4 0 0 0 1 0 0 y50 0 0 0 1 0 y60 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 10 0 0 0 0 y2 0 10 0 0 0 y3 0 0 1 0 0 0 y4 0 0 0 1 0 0 y50 0 0 0 1 0 43 y60 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 00000 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=10 0 0 0 0 01 0 0 0 0 0 01 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 01 x3 --angle deg x.true x.est(blended ax) 0 1 2 3 4 5 6 7 8 9 10 0 0.51 1.52 2.53 3.54 4.55 x2 --motor rate rad/sec x.true x.est 48 0 1 2 3 4 5 6 7 8 9 10 -10 -5 05 10 15 20 x4 --angle rate deg/s x.true x.est 0 1 2 3 4 5 6 7 8 9 10 0 10 20 30 40 50 60 70 80 90 u --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 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: 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 theoremhttp://hyper physics.phyastr.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 05 10 15 motor #3and #4 speeds (rps) vs pwm cmd 0->255,255->0 straight line approximation Dead Zone +-15 w__max=11rps at 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 noload) -2 02468 10 -125 -100 -75 -50 -25 0 25 50 75 100 125 Figure F2 vex pwm output 256 cnts per 1ms voltsacross motor 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_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 Modeling a Two Wheeled Inverted Pendulum Robot Download this Document for FreePrintMobileCollectionsReport DocumentReport this document?Please tell us reason(s) for reporting this document Spam or junk Porn adult content Hateful or offensive If you are the copyright owner of this document and want to report it, please follow these directions to submit a copyright infringement notice. Report Cancel . .This is a private document. Info and Rating Reads:3,623Uploaded:04/05/2010Category:How-To Guides/Manuals>GadgetsRated:Copyright:Attribution Non-commercialWhite paper applying control theory to the stability of an inverted pendulum robot built mostly from Vex robotic kit parts. Includes MATLAB and OC... (More) White paper applying control theory to the stability of an inverted pendulum robot built mostly from Vex robotic kit parts. Includes MATLAB and OCTAVE code for nonlinear model. (Less) .

firstrobotbalancePendulumgyrovexInvertedAccelerometerlinear quadratic regulator(more tags)firstrobotbalancePendulumgyrovexInvertedAccelerometerlinear quadratic regulatorvamfun(fewer) .Followcdsiegert..Share & Embed Related Documents PreviousNext p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p. p..More from this user PreviousNext 52 p. 25 p. 6 p. 59 p..Recent Readcasters .Add a Comment SubmitCharacters: 400

This document has made it onto the Rising list! 10 / 16 / 2010 ..Print this documentHigh QualityOpen the downloaded document, and select print from the file menu (PDF reader required). Download and Print .You Must be Logged in to Download a DocumentUse your Facebook login and see what your friends are reading and sharing. Other login optionsLogin with FacebookSignupI don't have a Facebook account .email address (required) create username (required) password (required) Send me the Scribd Newsletter, and occasional account related communications. Sign Up Privacy policy You will receive email notifications regarding your account activity. You can manage these notifications in your account settings. We promise to respect your privacy. Why Sign up? Discover and connect with people of similar interests. Publish your documents quickly and easily. Share your reading interests on Scribd and social sites. ..Already have a Scribd account? email address or username password .Log In Trouble logging in? .. Login SuccessfulNow bringing you back... Back to Login Reset your password Please enter your email address below to reset your password. We will send you an email with instructions on how to continue. Email address: You need to provide a login for this account as well. Login: Submit Upload a Document Search Documents Follow Us! scribd.com/scribdtwitter.com/scribdfacebook.com/scribdAboutPressBlogPartnersScribd 101Web StuffSupportFAQDevelopers / APIJobsTermsCopyrightPrivacy.Copyright 2011 Scribd Inc.Language:English.Choose the language in which you want to experience Scribd:EnglishEspaolPortugus (Brasil).