EE 122 Final Project: Balancing Robot Ian Dimen, Andrew Wong 12/6/2006 Introduction We designed a two-wheeled balancing robot

to investigate the capabilities of analog control on the classic inverted pendulum problem. The problem can be described using the canonical pole on the cart picture Fig. 1. In this picture, a car tries to keep a pole upright ( = 0) by moving back and forth in the x-direction. The arrows shows force direction, particularly how accelerating in the positive x-direction results in an oppositely directed force on the pole. For our project, our robot's body is the pole, which is supported by the rear two-wheels of an upright RC car chassis. The balancing pole/robot problem is a classic unstable system and requires feedback to keep the pole upright. To see this, we can derive the transfer function of voltage across the DC-motor to angle of the pole:

Fig. 1. The Problem

Eq. 1. Transfer Function for system

Here, a and b are constants related to characteristics of the DC-motor and mass and length of the pole. We see there is one unstable pole in the right-half-plane.

potentiometer

sensor ref Xref

Control x Control Driver CAR

xsensor
optical encoder
Fig. 2 Block Diagram of Balancing Robot

The design of our robot is explained by the block diagram in Fig. 2. Our robot ('CAR') is driven by a PWM controlled H-bridge ('Driver'). The PWM is controlled by the summed output of two control blocks, one controlling the angle of the pole, the other controlling position/velocity. Signals into each control blocks are fed by sensors: a potentiometer mounted on the bottom of the robot to measure angle, and an optical encoder viewing the motor gears to measure velocity. ref and Xref are both zero for our application. Control If we look at the transfer function in Eq. 1, we can draw the root locus of the system for angle:

Fig. 3 Root-locus of transfer function (normalized constant values)

Here we see that simply closing the loop over angle and increasing the gain of the error signal (ref - sensor) will not move the pole out of the RHP. While one could cancel the zero at the origin with a perfect integrator in order to bring the pole into the LHP, it was not obvious to us how to build this perfect integrator with analog components. Instead, we adopted a more intuitive approach to balancing the robot that would allow us to tune our control empirically. We implemented the control blocks for angle and position/velocity using PD (ProportionalDerivative) and PI (Proportional-Integral) controllers, respectively. This gave us four tuning parameters for the control circuit that could control different behaviors in system dynamics. Proportional in angle controlled how aggressively the robot tries to reach equilibrium. Derivative on angle controlled the amount of damping in small angle oscillations around equilibrium. Proportional on velocity corrects constant acceleration caused by constant angle errors. Integral on velocity provides a position measurement so that the car doesn't wander too far from it's starting point.

Circuits Control Circuit

Fig. 4. Control Circuit

The control circuit is show above (Fig. 4) and shows the PI and PD blocks. Our sensor outputs were signals from 0-5V, thus at the beginning of each control block a non-inverting amplifier with offset of 5V rescaled the signal to -5 to +5V. From Fig. 1, we want a positive motor control for positive angle offsets. This pushes the car in the direction of angle offset and pushes the pole in the opposite direction. Likewise, positive velocities should actuate an increase in velocity to correct the angle sooner. To simplify the circuit for the velocity control block, we negated the output of the velocity sensor on the PIC so that we actuate a positive velocity on negative vel. Vin. C was chosen to to be the same for both D and I, (3 F). Integration rate was 1/(100 k * 3 F) = 3.3, and the derivative rate (10 k * 3 F) = .03. The rates are not very important, as the gains will set this rate anyway. We used 20-turn 20k potentiometers for Rth and RD. A 1k resistor was put in series with RD to limit the gain during adjustments. Rv controlled the P term for velocity control, and RI for the I term. Both were 5K potentiometers. Note that Rv and RD control the amount of attenuation of the velocity and position signals. This was done to match scales with outputs of the angle control. Finally the signal control out is output from a trimming 10k potentiometer to that rescales the -5V to +5V signal to a 0-5V signal that the PIC can use.

Motor Controller H-Bridge We implemented an NFET H-Bridge to provide the 30A PWM drive to our motor. A bridge using only N-channel was chosen for ease of component selection and driver design. The highside switches are controlled by a ground referenced driver instead of a floating gate driver for simplicity of implementation, as well as unlimited duty cycle. The voltage for these drivers and for the low-side switch drivers is 15.5v, derived from the 5V rail using a simple boost converter based on the MAX629 (Fig. 5). The device is configured according to the datasheet.

Fig. 5. DC-DC Boost Converter

The motor controller consists of a H-bridge driven by PWM signals generated from a PIC. The PWM signal is only applied to one of the bottom switches in the bridge: the diagonally opposite switch remains on, selecting a current path. When the motor is reversed the bottom switches are shut off, the top switches switch state, and then the bottom switch diagonally opposite from the top switch which is now on recieves the PWM signal. We chose to implement the PWM in a PIC (PIC16F72) for flexibility and reliability. As a state machine, the PIC is able to easily avoid failure states, such as turning on both the top and bottom switches on one side. The microcontroller is also able to perform simple signal processing, such as the introduction of a dead-band near zero signal, and the addition of a small positive offset to overcome motor friction. This PIC has a dedicated PWM output and configurable CMOS outputs to control the state of the H-bridge. In Fig. 6 we show our circuit. All transistors are 2N2222 and MOSFETS are n-channel SUP85N15-21. Each of the four inputs to the gates in the H-bridge are driven by totem pole circuits meant to amplify the PWM output from the PIC as well as the voltage outputs of the directional control (Slow). The bottom totem pole drivers draw 1mA from the input and source 400mA, and the top drivers draw 100uA and source 40mA. This disparity in driver currents is to provide fast enough switching times for the bottom PWM MOSFETs to reduce their power dissipation, while not dissipating unnecessary current to drive the top directional MOSFETS (which dont need to be switched quickly). QG = 76 nC for the MOSFETS, so we need to charge the gate capacitor 76nC on the order of 300 nS to achieve reasonable power consumption. To satisfy these constraints, we chose R4 = 50 k,

R5 = 15 k, R6 = 15 k for the top two totem pole circuits, and R1 = 5 k, R2 = 2 k, R3 = 1.5 k for the bottom two totem pole circuits.

Fig 6. Motor Controller H-Bridge and Drivers

The PWM signal is controlled by the analog signal into pin 6. This signal comes from our control circuit. The signal is centered at 2.5V, and the range is 0-5V. Since it is then converted into an 8bit signed binary number, the signal range translates to a code range of -128 to 127. Codes from 3 to 3 are translated as zero, which introduces a dead band. The top directional switches change state only when the signal leaves the deadband, which prevents them from switching excessively when the signal is near zero. The control signals to the top switches are active high CMOS, which is converted into a base drive current by the resistors R4. The control signal from the PWM is likewise active high CMOS, and is converted to two identical base drive signal currents for the two PWM drivers by resistors R1. This provides the same PWM signal to both halves of the bridge. However, only one switch is to pulse-width-modulate at a time, and the other is to be off. To achieve this, two open drain outputs of the microcontroller are connected to the inputs of the drivers in parallel with the PWM signal current as PWM control outputs. Putting a logic low on these PWM control outputs sinks the signal current and redirects it away from the drivers, disabling them. Either or both PWM switches may be shut off in this manner. A logic high (tristate enabled) allows the signal current to reach the switches.

Initially, we designed our motor controller to provide 19.4V to the MOSFET gates to make sure they were turned all the way on. This is only 0.4V below our rated Vgs. We also had not included a dead-band in the motor controller, nor had we placed appropriate filtering capacitors around the H-bridge. When we first powered it up under no-load, the wheels spun well, but quickly the output power dropped and it was apparent we were burning FETS. We discovered that the top switches were developing a short from gate to source, and developing a high resistance short (some ohms) from source to drain. We were perplexed at first, as these switches never have more than 12.4V Vgs (only the bottom switches experienced the full gate drive voltage). We then discovered that feedback from the H-bridge to the analog input, coupled with a 10kHz sampling frequency were driving the top switches into a 10kHz oscillation near zero commanded power. To fix this problem we fixed the sampling rate to the PWM rate (820Hz), and introduced the 7count deadband around zero command. Just in case gate overvoltage was present, we also added capacitors between all nodes of the power circuitry, reduced the driver supply rail to 15.5V from 20V, and added clamp diodes to prevent the gates from going above the driver supply rail. Although the top MOSFETS still get a little warmer (~110oF) than the bottom ones, perhaps due to their lower gate drive voltage, we had no more problems with the motor controller after these fixes. Since the current provided to the motor is on the order of 3-30A, the H-bridge circuit was built directly on perf-board to prevent damage to the bread boards. For each of the MOSFETS, we screwed heatsinks to the drain tabs. We noted that the heat sinks were very heavy and could potentially break the leads on the MOSFETS, so we epoxied the heat sinks to the perf-board. Despite this, a robot crash caused the motor controller to smack into the ground, and the circuit board broke around the heat sinks. Luckily, all of the connections to the MOSFET terminals were made with heavy guage wire instead of circuit traces, and none of the connections broke. In the future, this circuit should be built on an FR-4 or equivalent glass board instead of Paper Phenolic/Synthetic Resin, which is really only appropriate for low vibration stationary consumer devices. Charge Pump In order to provide our controller with an analog velocity signal from the wheel encoder, the CMOS-level quadrature signal Fig. 7 has to be converted to a DC voltage proportional to its angular velocity. We chose to implement a charge-pump V-F converter, since this would relegate the digital to analog conversion to actual analog circuitry, and leave the microcontroller free to act as a state machine to detect the quadrature direction.

Fig. 7 quadrature signal

The quadrature signal changes cyclically through four states [00 10 11 01], and the order of the vector represents the direction of rotation. In order to decode the signal, we have written an

assembly routine which executes an analogous state machine directly on the bit lines from the quadrature signal. The diagram in Fig. 8. shows the layout of the state machine. In any given state, the code is capable of branching along one of the two vectors highlighted in red, to a neighboring state. When the condition shown in the diagram is met, the processor will branch along the vector: both vectors are being checked alternately, so if both bit lines change state simultaneously between samples the processor will default to a random choice of the two vectors Each clockwise vector is treated as a positive position step and will toggle the Positive Pulse line to the charge pump, as shown in Fig. 9. Each counterclockwise vector will toggle Fig. 8 State Machine the Negative Pulse line. Thus, for every two state changes the respective Pulse line will cycle once. The capacitors in series with those lines, the bucket capacitors, will change voltage by a set amount and back again for each cycle of their respective inputs: this is because the diode and transistor conduct when the bucket capacitor terminal rises 600mV above or below the voltage on the 1uF accumulator capacitor. This means that for each cycle of the input, the bucket will inject a set amount of charge into the 1uF accumulator through the diode, and then be recharged with the same amount of charge through the transistor. Since the transistor amplifies current and thus charge drawn from the accumulator through the base by a large factor beta, the charge drawn from the accumulator to recharge the buckets is negligible compared to the charge delivered to the accumulator through the diodes. This charge may be formulated as Qaccum = (11/beta)Cbucket(Vpulse VBE VD)nPulses, or approximately Qaccum = Cbucket(Vpulse 1.2)nPulses. The current into the accumulator may be calculated as Ipump = Cbucket(Vpulse 1.2)fpulses, simply by differentiating the previous equation with respect to time. The output voltage is then RLCbucket(Vpulse 1.2)fpulses, where RL is the 50k voltage divider equivalent resistance plus any external loads. Finally, the output RC filter removes high frequency components generated by the stair-case shaped waveform.

Fig. 9 Charge Pump

In the case of our charge pump, there are two anti-parallel pumps attached to the same accumulator: the Positive Pulse and the Negative Pulse pumps. These pumps deliver charge packets of opposite polarity to the accumulator, so with the 2.5v quiescent pump output, the 05V range may be used to represent bipolar signals by selecting the pulse line of the correct sign. Since the full scale voltage change is 2.5v across 50k, the pump needs to deliver 50uA of current full scale. With the indicated component values and a 5V power supply, this works out to a 600Hz full scale pulse frequency. The pulse frequency is twice the encoder optical frequency, due to the fact that the state machine transitions four times per cycle and is then divided by two

in the bit-toggling operation. Our encoder remains un-characterized as to counts per rotation, but we get about 500mV output from the charge pump at a robot speed of 1m/s. Note that the time constant of the accumulator is 50ms, giving a cutoff frequency of 3Hz. The cutoff of the RC output filter is 15Hz, so as to ensure the 3Hz pole is the dominant one. The output filter mainly minimizes the sharp edges and corners of the staircase waveform, and reduces radiated noise. In developing the charge pump, we found leakage current to be of crucial importance. We were surprised when the circuit, initially showing no offset, began to drift away from the nominal 2.5V output at zero speed. We initially thought device leakage current was the culprit, but the problem persisted with the active components removed. This was surprising, as it indicated a leakage current in the tens of uA: we saw offsets of up to 2 volts, and they drifted up and down: ionic conduction was suspected. The culprit turned out to be bad solder, with a conductive rosin. Yes, you heard me correctly, the flux deposited on the board from the core of the solder during normal soldering was conductive: so conductive, that when a multimeter probe was placed in the waxy substance near a trace, contact to the trace itself showed a resistance of 1.7Mohm. This was with only a point contact from the multimeter probe, and with 1mm of flux between contact and trace. This solder is extremely useless, and if you find it around the lab please throw it out! Sadly, we do not remember the appearance of the culprit. To remedy this problem, we rebuilt the charge pump with space-wiring: only the component bodies themselves touch the sensitive accumulator node. This version on the circuit performed entirely satisfactorily.

Tuning the robot The advantage of our control circuit is the ease in tuning the robot. In the figure below we show the effects of tuning the proportional term in the angle control block. Angle, velocity, derivative of angle, and output control signal are plotted.

a.

b.

Fig. X Output of robot angle, velocity, angular velocity, control output. (Note velocity has baseline of 2.5 V, angle, 0 V). a) Ptheta gain = 4.5, Dtheta gain = 12.8. b) Ptheta gain reduced to 3.2 to reduce overshoot.

In the figure above, we show the response to an initial step in angle. The with a high proportional angle gain the robot overshoots equilibrium and oscillates in an attempt to correct itself. By lowering the proportional gain, we make the initial impulse on the motor weaker and allow the derivative control to do its job in lowering the motor speed towards equilibrium. Note that the above could also be achieved by turning up derivative to compensate for the increase in proportional term, but it is better to keep gains low so there is more room to tune when the velocity loop is closed. In the experiment above, the PI terms in the velocity control were turned off. This is why the velocity error slowly increases after the robot corrects the step input.

We adopted the following steps to tune the control circuit: 1) tune P and D in the angle loop to eliminate overshoot and oscillations to a step response. 2) increase I term in velocity control to reduce range in motion during angle correction, also increase D in angle loop to compensate loss of damping 3) increase P term in velocity control to reduce range of velocity errors. 4) iterate. What we noticed was that the deadband in the PIC controller affected the dynamics of the control loop. For small angles, the motor does not turn with small control signals. This means that during after the time of free fall in this deadband, the controller has to work harder to actuate a correction signal. Now supposing the angle is corrected perfectly, and velocity is stopped, the position will be offset by some amount that is proportional to the size of the deadband. Conclusion Our robot performed very well in maintaining balance, or a stable angle, over a long period of time. The position tended to drift to one side over many slow oscillations (about 1m / minute). This drift was due to the integration error of our integrator, which had no zero trim. The oscillations hints that the system is still unstable when looking at the robot's position. We suspect a nested architecture over angle and velocity might be able to solve such a problem, but it was not clear how to build the components necessary to move unstable poles to the LHP with this approach.