After running across David Anderson's impressive balancing robot on the Internet, I decided it was too cool, and I had to have one like it.  So, shamelessly ripping off his great idea, I created my own version of this two-wheel balancer.  The basic form is similar to his, and other balancing robots.  However, the actual implementation is slightly different.  Wanting to keep the cost down, and not having a home machine shop, I used wood and MDF board rather than metal and polycarbonate.  Rather than using precision metal gears, I chose a cheaper belt drive.  All the electronics were homebuilt, except for the CPU board itself, although this too has a lot of custom circuitry.  All the software was written by me, except for some parts taken from the autopilot project, and the Fire Marshall Bill robot project.

(It's still wearing training wheels, not because it falls over, but to avoid having to lay it down gently each time I shut it down or reflash the firmware.)

Here's a summary of the specs:


The wireless link has become especially useful, now that the bot is able to balance and move around on its own.  The Radiotronix modules essentially extend my serial port over a short range radio link.  On the PC side, I'm using a regular USB-to-serial adapter, which I modified to include a Radiotronix module inside (click here for details).  The bot has an identical module connected to its serial port.  The serial link supports a command shell, which allows me to control some vital parameters in real time, such as filter coefficients, robot position, velocity, etc.  It also has a streaming mode, which allows me to collect real-time data on the robot's performance, for later analysis.

bk-bot is powered by

Here's an example of the current shell commands: bkbot command shell: pwm <chan> <percentage>     - set pwm value pwmfreq <value>             - set pwm frequency streaming <on|off>          - stream measurements adc                         - read adc values encoders                    - print angular encoder values lcd <text>                  - write text to the lcd coef                        - show the filter coefficients bkp <val>                   - set the Kp constant for balancing bki <val>                   - set the Ki constant for balancing bkd <val>                   - set the Kd constant for balancing pkp <val>                   - set the Kp constant for position pki <val>                   - set the Ki constant for position pkd <val>                   - set the Kd constant for position k0 <val>                    - set the state-space K0 constant k1 <val>                    - set the state-space K1 constant k2 <val>                    - set the state-space K2 constant k3 <val>                    - set the state-space K3 consta hkp <val>                   - set the Kp constant for heading hki <val>                   - set the Ki constant for heading hkd <val>                   - set the Kd constant for heading mkp <val>                   - set the Kp constant for motor mki <val>                   - set the Ki constant for motor mkd <val>                   - set the Kd constant for motor dbcompf <chan> <val>        - set the motor deadband compensation (forward) dbcompr <chan> <val>        - set the motor deadband compensation (reverse) tiltsense <actual|kalman>   - use floor feeler or kalman tilt balance <on|off>            - enable or disable balancing motor <chan> <vel>          - set motor velocity in percentage motorstep <chan> <vel>      - test motor step response motorstats                  - motor controller statistics move <val>                  - move forward or backwards tilt <val>                  - force a specific tilt turn <val>                  - turn in degrees beep <val>                  - beep for val msec isp                         - jump to monitor reboot                      - hardware reboot tasklist                    - task debug info bkbot >

The robot uses optical encoders on the motors, using a home-brew technique similar to what others have done.  I used reflective optical sensors from Omron, and superglued a simple hex nut as the rotating target.  The flat sides of the nut reflect the light well, resulting in six pulses per revolution.  The motors have a 19.5:1 gear ratio, and the belt drive is 3.2:1, resulting in a 62:1 drive ratio.  This results in about 375 pulses per wheel rotation, or about 0.03 inch resolution. The backlash in the gears is much greater than this.

You can see that the quadrature output is nice and clean.

There are several parts to the control system for this robot.   The accelerometer and gyro measurements are fused in real time by a 2-state Kalman filter.  This filter, runing at 100 Hz, combines the best features of the two measurements and helps to eliminate gyro bias and drift over time.  Accelerometer measurements are filtered by a low-pass filter with about 10Hz bandwidth.  Enhancements were added to reject accelerometer measurements if the measured gravity vector indicates a significant linear acceleration.   The kalman filter produces a fairly accurate estimate of the robot's tilt, even when driving around.

There are separate controllers for each of the motors.  They take the requested motor velocity and drive a PWM signal to the H-bridge to move the motors as quickly and accurately as possible.  To deal with non-ideal motor behavior, the motor controllers comprise a PID loop, plus feed-forward deadband compensation.  The motor controllers also determine the motor position and speed from the optical encoders.  A better motor controller would also consider motor current, and this may be added in the future.

This robot uses cascaded PID loops for balancing.  The inner-most loop is the motor controller running at the highest rate (125 Hz), followed by a balance PID loop running at 100 Hz, and an outer PID loop for driving, running at the slowest rate (10 Hz).  Another PID loop, decoupled from these, is used to control the heading by applying differential drive to each wheel.  The proportional balance coefficient controls the stiffness, while the integral coefficient allows the bot to get its wheels out in front of itself when slowing down.  Integral action also eliminates the steady-state error, and helps the bot get over bumps by increasing the amount of drive.  Derivative action helps to reduce oscillation.

An alternative control system is being tested which is based on an MIMO state-space controller.

Here's a block diagram showing the major components of the control system:

Performance

This graph shows the raw tilt and tilt rate, as read by the accelerometer and rate gyro.  It also shows the tilt as computed by the kalman filter.  This data was read from the bot while it was balancing on the carpet.  The PID tuning is still in progress, and as you can see from the graph, the bot is wobbling very slightly. 

(Green and pink are inputs, blue is the output of the kalman filter.)




Open-loop (uncompensated) motor response to a 50% step input is shown below.  Notice the large steady-state error between requested and actual speed.  Here the PWM output is simply equal to the requested speed.  The performace is even worse for low speeds, as the motor doesn't turn at all until the PWM value exceeds about 7%.  The oscillation is due to uneven friction in the gears or bearings.



A motor speed control PID loop was added to compensate for the non-ideal motor behavior.  Each motor has its own PID loop running at 125 Hz, using velocity feedback computed from the quadrature counters.  Integral anti-windup is enabled when PWM output saturates at 100%.  A deadzone compensation algorithm was added to deal with the deadzone non-linearity. 

The improved, closed-loop step response of the motor is shown below.  Settling time is less than 0.5 second, with less than 2% overshoot.  The PID coefficients were determined experimentally to be [3.0, 6.0, 0.25]. 



The motor controller is especially useful at low-speeds, where the motors exhibit the most non-linear behavior.  When the bot is balancing, most of the motor movements are low-speed.  Here's a step response to a -5% request:



The drivetrain is not ideal.  It has backlash in the gears and the belt drive.  A backlash compensator may be required in the control loop.  In addition, the motor torque induces a rotation in the bot itself which tends to tilt the bot in the direction opposite to that which it is leaning.  This latter effect should work to our benefit.

After the motor controller was added, the actual motor speed, under load, closely tracks the requested motor speed:



The bot responds well to various disturbances: pushing against the side, or down on the top, or running over or into obstacles.   It turns well, using the heading PID loop.  It remains reasonably steady on both carpet and hard surfaces. 

At this point, driving is commanded either with the remote control, or with shell commands.

Videos

Video of stable balancing on carpet, and turning around.  This is low-resolution, with better video coming soon.
(It is still wearing training wheels in the first two video clips.)

     

Source code

Here's a link to the early firmware.

---

bkuschak [at] yahoo.com
Updated: Feb. 16, 2007