Fresh look at self-balancing robot algorithm

Self-balancing robot is a cool project, which many makers like to build somewhere during their journey through the world of robotics.

In most cases you can find on the internet there are three different types of motors used in a self-balancing robots:

1. stepper motors
2. DC motors
3. DC motors with quadrature encoders

Options a) and b) do not give you feedback on the distance (even in case of stepper motor it is easy to lose steps), so we’ve decide to use DC motors with encoders. Robot keeps balance like this:

What you see is basically a telepresence robot which can be controlled through the internet from anywhere with anyone via shareable link.

Now let’s go deeper into the story.

Existing self-balancing robot constructions and our way to make our own

90%+ of all self balancing constructions found on the internet are based on Arduino, so it’s rather hard to expand them with new functionalities because of lack of computing power and a single infinite loop with everything done within it.

While creating our own self-balancing construction, we had the following assumptions in mind:

1. using DC motors with encoders
2. using MPU9250 inertial measurement unit, but not raw accelerometer and gyro data, but complete 3D orientation data from DMP (Digital Motion Processor) inside this chip
3. creating an elegant control algorithm that is responsive, resilience to interference, easy to understand and easily scalable for other mechanics with only a small tune up of PID parameters
4. creating an easy to use class for robot control algorithm with a simple API and control algorithm working as a “background task”, so the robot firmware will be easy to expand.
5. make it open source to help people reuse and modify it in their own projects

At first let’s start with the hardware we used.

Our robot is based on Husarion CORE2 robotic controller that has integrated interfaces for up to 4 DC motors with quadrature encoders that are handled in “hardware” (there’s a dedicated timer working in quadrature encoder mode), so it doesn’t waste processing power of CPU to handle encoders. Husarion CORE2 controller is programmed thanks to easy API in C++ 11 provided by open source programming framework available at https://github.com/husarion/hFramework (MIT license) with Arduino compatibility layer (which is not used in this software, but could be if you are an Arduino enthusiast 😊 ).

What are the results? Just take a look

Our goal was to prepare not only the control algorithm but also provide easy to use API. It looks like this:

Using this API we’ve create our telepresence robot. Here are the main features of the final project:

Smooth driving

Resilience to disruptions

Servo controlled leg to start/stop balancing

Intuitive web user interface — you can control this robot from a web browser with arrows on your keyboard or built in joystick in the user interface

Stabilization algorithm explained

After a lot of trials and errors we have finally created a control algorithm based on 4 PID regulators:

It may look complicated at first glance but it is quite easy to understand. Let’s analyze it from right to left:

At a hardware level we’ve got the following interface provided by motors and sensors:

1. Power and turning direction that could be applied to a motor (API call “motorRight.setPower(1000)”, means 100% PWM signal and forward rotation, “motorRight.setPower(-500)”, means 50% PWM signal and backward rotation), information about angle in degrees (0° — robot is in vertical position to ground, >0° — robot is leaning forward), and encoder counter information for each of the wheels (>0 — robot moved forward by a positive number of “encoder ticks”).
2. The first level of PID regulators to control power applied to each motor to achieve the target speed (measured thanks to encoder signal and known control loop frequency). In the previous version of software we haven’t used these PID regulators which led to inability to follow straight line while driving, and instability in turning — each of the motors have a different electrical characteristics for forward/backward rotation. Thanks to this level of PID regulators we can control not the power, but a rotation speed of each motor.
3. PID angle regulator — keeps desired angle between robot chassis and a ground based on feedback from MPU9250. PID angle receives an interruption from a MPU9250 every 20ms with fresh angle data. Thanks to the interrupts we don’t waste processing power of thee CPU on asking the chip whether new data is present (we use RTOS, so CPU doesn’t care about this task until new data from the sensor is available).
4. PID speed — it does similar work to the motor speed regulator but controls speed of the whole robot, not only a single motor. Imagine that this regulator controls robot angle to ground to achieve a desired speed, because if the robot tilt is positive, it has to accelerate to prevent falling down. If the final speed is reached, robot automatically backs to the 0° angle to the ground (based on Newton’s first law of motion).

Here’s a sample of the stabilization algorithm code below:

How can you get started?

Full source code is avaiable at

https://github.com/husarion/self-balancing-telepresence-robot .

I hope you find this article helpful and wish you a great self-balancing-robot-building experience 😊 . Your feedback means a lot, so feel free to ping me at dominik.nowak@husarion.com or place a post on our community forum https://community.husarion.com/ if you feel like sharing your thoughts. Take care!