Balancing g Robot Control MEAM 510 Fall 2011 Bruce D. Kothmann
Agenda Bruce s Controls Resume Simple Mechanics (Statics & Dynamics) of the Balancing Robot Basic Ideas About Feedback & Stability Effects of Proportional Feedback Two Key Observations About Integral Feedback Derivative Feedback Used to Stabilize Why PID Feedback of Arm Angle Won t Work Effect of Sensor Alignment Effect of Sloped Table What Will Work Some Implementation Issues Hall Effect Sensor for Wheel Angle Dealing with Accelerometer Noise & Rate Gyro Bias 10-Bit A/D and Amplifiers Sample Rate : Is Faster Always Better? Best Robots Invited to Demo in ESE 406 This Spring! MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 1
Inverted Pendulum MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 2
Inverted Pendulum Description M1 Microcontroller (PID Control) Encoder From Lego Motor 43362 EncoderGeek.com http://www.philohome.com/motors/motorcomp.htm MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 3
Ping Pong Poise MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 4
Ping Pong Poise Description This is a Fixed-Point Regulator Servo Motor Optical Voltage Divider M1 Digital Microcontroller (PD Control) MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 5
My Other Controls Design Experience MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 6
Simple Mechanics Part 1 : Static Equilibrium Q=Motor Torque Pin Shear Force 2r Weight F= Friction Normal Force l FBD of Body Weight Pin Shear Force Q=Motor Torque Sum Moments @ Center of Wheel Q rf Sum Moments @ Pin in Body Q mglsin mgl Sum Forces Parallel to Slope on Both Objects (Pin Shear Drops Out) sin M r F Mg Mg ml MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 7
Simple Mechanics Part 2 : Dynamic Instability l Weight Technically WRONG 2 but Qualitatively Sort of Close to Right Dynamics of the Body dt Q=Motor Torque d I Qmglsin Qmgl 2 Main Problem : Gravity is Like a Negative Spring If Pendulum Starts Falling, Gravity Makes it Fall Faster (Control Lawyers Call This a Dynamics Problem, Not Disturbance Problem) MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 8
Effects of Proportional Feedback 2 d Q K P I K 2 P mgl dt Proportional Feedback Gain Has To Be Large Enough to Create Net Positive Spring (Overcome Open-Loop Negative Spring) Beware of Destabilizing Effect of Delay (Due to Processing Time or Other Dynamics) Qt ( T) K ( t) P Position @ t+t Position @ t MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 9
Two Key Observations About Integral Feedback To Reach Steady State (Equilibrium), Input to Integral Feedback MUST Be Zero! This Is Why Integral Feedback is so Powerful! This Is Why Integral Feedback is so Peril-ful! Same Speed Requires More Throttle Example : Cruise Control Encountering a Hill Integral Feedback Throttle Angle Change Proportional Feedback Speed Change Integral Feedback is Very Often Destabilizing (Including in This Problem!) MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 10
Effect of Sensor Alignment Attitude Sensor Won t Be Perfectly Aligned with Line Between Hinge & CG Static Equilibrim (See Below) Cannot Achieve =0! Even Integral Feedback (Which Acts Like K P Infinity) Fails! Cart ALWAYS Slowly Drifts Away! Q K K mgl P M P K mgl K P P M Effect Exaggerated for Clarity M MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 11
Effect of Slope M r ml Static Equilibrium (Overall CG Must Be Above Wheel Contact Point) Don t Want =0! Integral Feedback of Cannot Work on a Slope! Could Achieve Static Equilibrium with Exactly Right Value of Proportional Gain Only, But This Would Not Be Enough for Stability! Q Mgr mgl K P Torque Due to Feedback Must Equal Torque Required to Stay Upright MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 12
Human Operator! What Might Work? Use Wireless To Allow Operator to Command Lean Angle On Level Ground, Operator Cancels Installation Misalignment On Tilt, Operator Commands Lean Into the Hill Sufficient to Use PD Only? System Not Stable Without Operator, But Instability Slow Enough to Be Easily Compensated (Many Airplanes & Bicycles Are Like This) Use Wheel-Angle Feedback! Q K K P W Probably bl Want Derivative of Wheel Angle Too for Stability Arbitrary Steady Torque Can Be Achieved by Small Position Drift (Wheel Angle Change From Power-On Condition) This Fixes Both Installation Misalignment & Slope Effects! If Gains Make System Stable (You Need Equations to Know for Sure), Required Position Drift Happens Automagically! Hall Effect Sensor Can Measure Something Like (); Maybe Okay to Use PD Feedback on This? MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 13
Measuring Body Angle Accelerometer Sees Gravity as Negative Acceleration (General Relativity) But Accelerometer Also Sees Local Linear Acceleration (Due to Wheel Movement & Angular Acceleration Times Moment Arm) Accelerometer Has Noise, Especially at High Frequency 2 d a awheel r gsin 2 dt Body Angle Can Also Be Estimated by Integrating Angular Velocity, But This Will Drift (Because Angular Velocity Won t Read Exactly 0 at Rest) t (0) ( ) d 0 a MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 14
Complementary Filter : Combine a & Basic Idea Use Integral of angular velocity at high frequency (where a is noisy) Use (-a/g) at low frequency (where drift of angular velocity is bad) Theta = High Pass Filter Of + Integral of Low Pass Filter Of (-a/g) Need Digital Implementation of Integral & Filters Integrate High-Pass i i1 it ˆ ˆ i i1 i i1 Combine ˆ ˆ i i i Low-Pass ˆ ˆ 1 / i i 1 a i g MEAM 510 : Balancing Robot Control BDK : 2011-10-26 Page 15
Miscellaneous Implementation Issues Scaling of accelerometer voltage into A/D: you really only care about +/- ~5 deg, so make sure that range is 0 to 5 volts so you make good use of your 10-bit A/D (1024 Values) build a simple op-amp circuit? Very high processor speeds may cause problems, because digital filters may require very high precision. Also, digital filters are easiest to design with a fixed frame rate. MATLAB has a very convenient C2D function for converting analog filters to digital. Do you need to go faster than ~1000 Hz? I told a couple of lies earlier motor friction might allow proportional feedback to work on level ground & carpet might allow equilibrium on small slopes! But these effects are very unreliable (low robustness) MechaMiniSegway Notes BDK : 26-Oct-2009 : Page 16