The epic:

Build a robot-car with ‘a certain level of autonomy’.
Explore possible options for computer vision control.
Explore simple options for mapping and localization.
Preferably build a 2-wheeled differential robot.

In Reading Signs you’ll find a first exercise with computer vision control.

In turn this post offers a very basic approach to build a balancer and covers:

My experiences with different sorts of hardware components
A simple control logic
Control loop timing experiences and semi-automatic tuning

The video shows the first 3 versions of the balancing robot:

The basic balancer ran by an Arduino Uno and controlled by an Android app
Two extended versions loaded with ultrasonic sensors and controlled by a Raspberry Pi

(The appearance of the bot is constantly changing due to my experiments)

A future post will cover distance reading, obstacle avoiding and odometry experiments.

Step 1: Hardware

I experimented with several kinds of IMU’s, motors, motor drivers, wheels and power sources.

The prototypes shown in the video contain:

MPU6050
Pololu Low Power DC motors (2282: 590 rpm, 9.68:1, 17 oz-in)
Hall encoders 464.64 cpr (48 cpr after gear )
Pololu wheels 120 x 60 mm (Wild Thumper)
Arduino UNO
Seeed Studio 4A motor shield
Dagu bluetooth module (HC06)
Raspberry Pi 2B+
2x UltraBorg board for reading ultrasonic sensors on the Raspberry
2x DFRobot LiPo 7.4V battery
HOBBYWING 3A Switch Mode UBEC 5V 6V

Inertial Measurement Unit (IMU)

The robot in the video uses a basic and very low cost module based on the InvenSense MPU6050 chip. I also tried several SeeedStudio modules and versions based on the MPU9150 and MPU9250. The latter chip is named 10DOF (Degrees Of Freedom) for it adds a barometer (single axis) to the gyroscope (3-axis), accelerometer (3-axis) and magnetometer (3-axis).

All 3 chips have a Digital Motion Processor (DMP) on board. There’s a big advantage in using this processor. It saves a lot of coding on the calculation of the vertical angle (sensor fusion and filtering). Secondly it has a build-in calibration routine. And last but certainly not least: it saves use of the cpu and memory of the Arduino.

The InvenSense documentation, even in their ‘Developers Corner’, is not helping much if you want to use DMP in combination with an Arduino. Jeff Rowberg has published a great hack for the MPU6050 registers here This library works only for the MPU6050 and MPU9150. The registers of the MPU9250 are configured differently. While testing with the MPU9150 chip, DMP occasionally stopped working (could be the chip). Finally the MPU6050 gave the best results. (Other advantages: these modules are the smallest, thinnest and very low priced.)

If the MPU6050 produces erroneous data, one might want to calibrate the sensor x,y,z-offsets of the chip. An Arduino sketch for calibrating offsets can be found here

I experienced that tuning the offsets improve the accuracy at small angles but also decrease the accuracy at large angles. At the end-of-the-day the chips default offsets worked best most of the times.

Motors, encoders, motor drivers and wheels

Motors I used (all Pololu):

High Power Micro no 1101, 990 rpm, gear: 100.37:1, stall torque: 30 oz/in
High Power no 2271, 990 rpm, gear: 9.68:1, stall torque: 39 oz/in
High Power no 2274, 210 rpm, gear: 100:1, stall torque: 80 oz/in
Low Power no 2282, 590 rpm, gear: 9.68:1, stall torque: 17 oz/in

Except for the micro version, all motors come with hall encoders attached. They produce an easy-to-work with resolution of 464 to 2248 ticks depending on the gear ratio. For the micro motors I used optical encoders producing 48 ticks.

In my view for indoor use torque is less important. Also I experienced the high power versions as harder to tune. They have a larger stiction (‘dead power zone’ = amount of power needed to start moving) and therefor I found it harder to adjust power at tiny differences in balance (i.e. small vertical angles). Rpm is directly related to a maximum speed. In my view also of lesser importance for building a balancer for the first time. However a very low rpm makes it almost impossible to reach steady balancing. My advice: use average motors, preferably with around 600 rpm.

Overall the DC motors, although from the same series, showed different and non-consistent stiction. Building in differential control turned out to be essential. Maybe stepper motors will reduce this problem (they move until a position or angle is reached). Stepper motors also remove the need for encoders.

I’ve done tests with a L298N dual (2A) motor controller board and a Seeed Studio 4A motor controller shield. I prefer the latter for it’s a shield. Easy to mount and it takes less space. Furthermore: I found the shields 4Amp a better buffer for the stall current of the high power motors. (Note: Mind the pinout in my Arduino sketch. I experienced the shields wiring to be different from the Seeed example.)

Wheels can really make your life easy. I’ve used diameters of 4.2, 9 and 12 centimeter. Large wheels bring more stability. This also applies to the grip on the surface. My favorites: the ‘Wild Thumper’ wheels (12x6 cm).

Height and weight are also important for the behavior of the bot. A large weight will make the bot inert: reacting slow and therefor show large overshoots in the beginning. And certainly high powered motors are then needed. A higher center of gravity (COG) will make the robot more stable, but there are possible consequences too. Height, in combination with significant weight at the top, will also make the robot inert. So it’s all a matter of tuning. When the robot is balancing stable but you find it reacting slow when moving forward or backward, try reducing weight and/or bringing some weight down.

Power

This bot is powered by 2 LiPo batteries. One for powering the Arduino and the motors. One for powering the Raspberry and through that the UltraBorgs with the HC-SR04 ultrasonic sensors. I started with just one source for all components. When the motors were on ‘heavy duty’ they drew so much current that the ultrasonic readings became erroneous and even the RPi sometimes performed a reset. Using a separate LiPo for the RPi and Ultraborgs solved that, but I had to add a UBEC to get a steady current of 5V 3A .

Step 2: Logic & Coding

There’s a lot of information on balancing robots and/or the inverted pendulum available on the web. However I used a very simplified logic with the following assumptions:

1 - The angle by which the motor axis should turn, is (at least) the same as the angle by which the robot is falling. So try to control the angle of the motor axis instead of a distance and/or a speed. (Which, as said, might be easier to realize when using stepper motors.)

2 - For balancing, control of the angle and its derivative, the angular speed of the motor axis, is needed. This can be done by using a PD control loop. Not as common as a PID loop, but since the direction of the movement will be changing rapidly, the use of an integral factor is hardly usefull.

3 – To keep position the horizontal movement and speed have to be controlled. This can be done by translation of the sensor (encoder) output into a compensating angle. The control loop needed is also a PD loop for the same reason as the vertical control.

The results of 2 and 3 could simply be added, but it’s better to use nested (cascaded) control loops where the horizontal control loop produces a target angle for the vertical control loop.

Working by these assumptions saves the burden of calculations with the height and the mass (or even the linear velocity) of the robot (as said: those are important factors but can easily be tuned). This way it’s relatively easy to adjust height and COG of the robot without changing (much of) the coding parameters.

Keeping the robot straight faced is done through differential control (turning) and can be done independently of the horizontal and vertical control. The input could come from the encoders (i.e. difference in amount of ticks) or from the IMU (i.e. yaw data). The latter needs much less coding.

To wrap it up, for balancing one need to :

1 Translate the horizontal displacement into a vertical angle

2 Compensate the difference between that angle and the angle measured

3 Move the motor axis by this compensating angle

For translation into code some features of the robot are needed:

The maximum motor speed expressed in encoder ticks. This is the maximum of ticks the encoders can produce with a certain motor. In formula: RPM / 60 * ticks per revolution
The relation between encoder ticks and degrees. In formula: 360 / ticks per revolution

With these two we can deduce the relation between degrees and motor power (PWM value 0-255 ).

An Arduino sketch operates as robot 'firmware' with the following functionality:

Horizontal control depending on input from the wheel encoders
Vertical & differential control based on IMU input (pitch & yaw)
Bluetooth communication for tuning, output monitoring and motion testing
Serial communication with the Raspberry Pi

The sketch is available here. The android app for remote control and tuning has been made with the MIT App Inventor and the blocks can be found here. The Python scripts for communication and control between the Raspberry and the Arduino are available in the same repository. I won’t elaborate on those now for this post is meant for the balancing fundamentals only.

The Arduino sketch is well commented for explanation. So I will only touch on its functions shortly.

Initializing

The pins of the UNO are well occupied, leaving not much room for connecting sensors to the Arduino (pwm, interrupt and analog pins are sold out).

Some robot/motor constants to do the calculations (described above).

Slack or stiction constants. Gathered by trial and testing. I started with a sketch to obtain average values under no-load conditions. You’ll find it here. In practice I adjusted them significantly based upon observations while testing the balance and movements. These constants will suffice but are highly depending on the behavior of the motors. Skipping these amounts of power makes the robot more responsive at (very) small angles. But setting these values too high will create some challenges while tuning the control loops!

maxAngle

The angle where compensation is assumed impossible. It is used to determine if the robot is standing up or laying down.

Interval

A steady interval is essential for the control loops. There’s a ‘TimedAction’ library available on the Arduino site, but straight forward coding of one timer is just as simple and saves some memory.

pitchOffset

Obtained by tuning the IMU. Most simple way this can be done, is fixating the robot in an absolute 90 degrees position (measured by a digital level on top) and read the IMU-values.

cogOffset

Without this constant the control loops will also obtain balancing. The first prototype carried 8 ultrasonic sensors which made it a bit heavy at the front. As you can see in the video the control loops adjust this by letting the robot lean back a little. Using a cogOffset compensates the leaning and makes the robot a bit more agile in moving forward.

Serial.begin() and Bluetooth.begin()

Communication with the RPi is done through USB-serial. The SoftwareSerial wiring library is used for the Bluetooth communication with the Android app.

wdt_enable(WDTO_500MS)

When something went wrong in initializing the IMU, the Averdude Watchdog Timer is set which will force a reset of the Arduino.

resetRobot()

Initializes all variables (and stops the motors) every time the robot is laying down. This is helpful when picking up the robot.

loop()

As long as the robot is standing up all inputs are checked. When the interval has passed the control loops are executed. I’ve experimented with a different intervals. The optimum is depending of the sensors and type of motors used. Beyond 100Hz (< 0.01 sec) I experienced erroneous encoder data and the robot also acted a bit too ‘spicy’. At 50Hz the robot became ‘sluggy’. In most cases a 100Hz loop worked best.

horizontalControl()

Checks if the target distance has been made and adds a speed offset to the target ticks. The speed offset is the amount of ticks to be made during an interval at a certain velocity. If no distance and speed targets are set, the target amount of ticks is set at the rising of the robot. This enables the robot to return to its position (or new position if it was ordered to move for a certain distance).

differentialControl()

This is a bit special for the yaw output of the IMU is used here. The IMU returns - π <= yaw <= π (radians).

The getAngles() routine turns the reading into an angle in degrees on a 0-360 scale.

The resetRobot() routine sets the yaw-angle at the rising of the bot as yawTarget (basically an angle-offset on a 360 degree scale).

The difference between the last angle reading and the offset is treated as error.

Pretending a scale of 360 degrees (like on a compass) makes it easy to determine the smallest angle (=direction to turn). The outcome can simply be added to and subtracted from the amount of power send to the individual motors depending on the direction (left/right) to compensate and depending on the direction (forward/backward) the bot will have to move.

The next 3 routines are only executed when a command is received through Bluetooth or from the Raspberry in the getMessage() routine.

setSpeed()

Takes a percentage of the maximum amount of ticks the motors can produce in one interval (= RPM / 60 * ticks per revolution * interval in seconds).

setDistance()

Multiplies the distance to run in meters with the amount of ticks per meter (= Ticks per revolution / wheel perimeter in meters)

setTurn()

Adds the desired amount of degrees to the yaw offset, adjusts the new target to a 360 degrees scale and resets the turn command to prevent continuous turning.

The sensor and motor routines are pretty straight forward. The resetEncoders() routine prevents accidents caused by a ‘roll over’ of the encoders (encoder output is in integers).

The interaction routines check the serial and Bluetooth queues for new commands and if so handle those through a common case structure.

Step 3: Tuning the Control Loops

For obvious reasons PID-tuning is sometimes called a dark art. The process can be very tiresome and frustrating. Although there are a number of semi-structured methods of obtaining controller values, manual tuning is almost always unavoidable to get the bot balancing.

Some considerations that will help here:

This process requires patience and observation: if a gain value is changed, which behavior can be observed?
The Proportional gain stands for the Power of the compensation. I.e. the amount of motor power supplied.
Derivative gain stands for Damping the effect of the power supplied.
Keep in mind that it’s all about acceleration. Power supplied shortly delivers acceleration.
Start with both gains at zero and then start increasing the P(ower) with small steps. Stop when the bot stays more or less up by its own. Maybe too ‘shaky’ or ‘waggling’ but it should keep itself up for a couple of seconds. (Often referred to as ’steady oscillation’.)
If you really cannot get to steady oscillation, there’s probably a physical cause. Some possible signals are: uneven amplitudes (which could point to a cog, mass or height problem) or only moving in one direction (which could be a lack of motor power)
When the bot oscillates heavily (makes large, wild moves forward and backward) and the moves get wilder and wilder, the acceleration is too big because too much power has been supplied

When the bot starts running faster and faster in one direction to end in a crash, the bot cannot accelerate enough due to insufficient power supplied.
Steady oscillation implies constant overshoot like sitting on a swing. With adding D(amping) the overshoots will decrease (and finally fade).
Use small steps, keep the values as small as possible and observe the effects thoroughly.

Life becomes a bit less tough when the gains can be adjusted from outside the sketch (otherwise one has to edit, recompile and rebuild the sketch over and over again). I started with 2 trim pots to adjust the gains. I found them hard to use. Because I started with no idea about the gain values, I had to edit the sketch many times for a new scaling (?.???? : 1024) and without a direct visual feedback on the values, I had no clue of the changes I made by turning the pots.

The android app worked best for me: almost all constants can be changed by just entering a value. The top of the screen gives an overview of all current values. At the bottom of the screen one can get a graphical display of the error or the angle itself. Mind that such a display is just a delayed (Serial and Wifi) indication and not a real live feedback. The app can also be used for testing some commands (distance, speed, turns).

Once I had reached the ball park figures, I felt the need for some semi-automated testing tools because almost every change to the robot or its code requires more or less re-tuning. Especially working on the horizontal control loop required that several times.

I wrote a Python script that uses a coordinated ascent algorithm often referred to as ‘twiddle’ algorithm. An explanation of the algorithm by Sebastian Thrun (Udacity, Google, Stanford) can be found here

The script can be found here: twiddleVert.py

I tried to build the algorithm in the Arduino sketch. Although it just fitted into the available 32K of memory, it delayed the overall cycle too much. So the script is meant to run on the Raspberry and uses some other Python scripts:

control.py for sending commands to the Arduino and I2C communication with the UltraBorgs. If no Ultraborgs are attached the import and initializing could best be commented out. Otherwise UltraBorg.py is also needed.

messaging.py which is a threading class for the actual communication with the Arduino

linkingStorage.py which acts as shared memory between the main script and the threaded script

measurements.py for gathering, displaying and saving all test data.

Note:

The Python scripts will change due to further experiments. If you want to avoid changes clone the repository.

I think all fundamentals are covered by now and will hopefully help you in building your own balancer.

Enjoy!