Robust Two-wheeled Balancing robot

I wanted to share my two wheeled balancing robot. It’s my first robot and took way more effort than I anticipated. But It’s pretty robust against disturbances so I’m quite happy with the result. I’ll write up and open-source the design considerations and built process, and source code, in a blog post soon.

Height: 37cm / Weight: 3kg

Video 1:
Video 2:
Video 3:

Will write a blog post soon but for now, here is a summary of hardware and software design.


Cascaded PID controller. The outer loop runs a PID controller at 40Hz to control the speed, and another PID controller to control the steering angle (wheel RPM difference). The latter makes sure that the robot can drive straight, even if the two motors are not identical.

The output of the outer loop, is a target angle, that the robot needs to lean into to obtain the desired speed. This is the setpoint for the inner loop, which runs a PID controller to do the actual balancing at 400Hz, and takes input from a sensor fusion algorithm running at 400Hz too.

Sensor fusion runs at 400Hz on a Teensy 4.0. Attitude estimation is based on Madgwick’s complementary filter ( - note, this is not his older gradient based implementation that many still use.).

Output is sent over Serial to a second Teensy 4.0 MPU which runs the main balancing algorithm. This one communicates over Serial with a third Teensy 4.0 which runs the velocity and steering controllers, takes care of deadzone compensation, and communicates with the Sabertooth.

1 Like

Hi @v42 and Kudos for the effort!

Why so much MPU power? Was it necessary because you added all the Serial-latency to the PID algorhythms?
I once built a tiny BR based on a single Atmega328P. It balanced quite good, but I failed to add the RC- steering.

All that MPU power isn’t needed for now, but I’m planning to keep improving the bot over time. First by trying some more modern control techniques like LQR or MPC, which need quite a bit more computational power than simple PID controllers. I’m also planning to replace my quadrature encoders by high resolution encoders, and add some more peripherals (distance/audio/video sensing, etc.). In the end, having different MPU’s for each logical unit will make sense, even if it’s a bit overkill today :slight_smile:.
On the Serial communication: I don’t think it adds a lot of latency. With a Teensy 4.0 you can easily communicate at a baudrate of 2M.
This bot was mostly a learning experience for me, as I never touched a soldering iron before :).
Love your tiny BR btw!

1 Like

This is great! I am also working on a self-balancing robot and have also settled on using the Fusion library. I’m at the point where I have just integrated the library into my code, and have created a few development tools to help characterize performance and understand it better (simple RPY visualizer, etc.).

It would be extremely useful to see how you approached/used the different library parameters. The reason is the performance I am seeing with default settings in my system (based on an Arduino Nano 33 Ble Sense Rev 2’s Bosch BMI270/BMM150 sensors) is far from what I will need, and I am trying to determine if this is due to library parameters, update rate, axis mismatch, or something else.

Do you have the code published anywhere?

1 Like