Self balancing robot

Showroom for MicroPython related hardware projects.
Target audience: Users wanting to show off their project!
jeffm
Posts: 23
Joined: Tue Mar 10, 2015 9:11 am

Self balancing robot

Post by jeffm » Tue Apr 21, 2015 8:13 pm

I have just posted a video of my self balancing robot project using a Pyboard:

http://youtu.be/Onv2QZuZcC0

The frame is implemented in Makerbeam and it is powered by two 12v NEMA 17 stepper motors driven by A4988 driver boards. It uses an mpu6050 gyro & accelerometer.

To give smooth acceleration it uses a 10khz timer driven interrupt routine to pulse the stepper motors and the main balance control loop including complementary filter runs every 5ms. I was impressed that this sort of real time performance is achievable with micro Python.

Direction control is a very simple and cheap four button radio controller.

If anyone is interested, I will try to get the code up on GitHub this weekend and post the link.

I have really enjoyed using the pyboard, it is a really great tool for interfacing with hardware without having a huge OS getting in the way.

Thanks Damien

User avatar
dhylands
Posts: 3821
Joined: Mon Jan 06, 2014 6:08 pm
Location: Peachland, BC, Canada
Contact:

Re: Self balancing robot

Post by dhylands » Tue Apr 21, 2015 8:54 pm

Sweet.

User avatar
bmarkus
Posts: 111
Joined: Tue Oct 21, 2014 5:58 am

Re: Self balancing robot

Post by bmarkus » Wed Apr 22, 2015 5:44 am

I love it. Waiting for GitHub code :)
Tiny Core Linux (piCore) developer
HAM radio call: HA5DI (Béla)

User avatar
pythoncoder
Posts: 5956
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: Self balancing robot

Post by pythoncoder » Wed Apr 22, 2015 6:41 am

I'd be very interested to see that, too. I've been trying to do the same thing, using a chassis of my own design. At present it's tethered but my intention is to have wireless control. But I can't stabilise the thing for the life of me and for a couple of months it's been gathering dust in the corner of my room :( It balances for a while before accelerating off on a mission of its own. Are you using a PID controller? Do you use Kalman filtering or complementary filters for processing the gyro and accelerometer data? How do you stabilise position as well as angle?

I've read numerous web articles about this and tried a variety of solutions but to date have made no progress.
Peter Hinch
Index to my micropython libraries.

MattMatic
Posts: 13
Joined: Wed Apr 22, 2015 1:44 pm

Re: Self balancing robot

Post by MattMatic » Wed Apr 22, 2015 2:07 pm

Very nice!!

Would be pleased to see the MPU-6050 support code ;)

User avatar
pythoncoder
Posts: 5956
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: Self balancing robot

Post by pythoncoder » Thu Apr 23, 2015 5:34 am

There is a well tested driver for the MPU-9150 here
https://github.com/turbinenreiter/micropython-mpu9150
It would probably be adapted or might even work as it stands: the 6050 is similar but lacks the magnetometer.
Peter Hinch
Index to my micropython libraries.

jeffm
Posts: 23
Joined: Tue Mar 10, 2015 9:11 am

Re: Self balancing robot

Post by jeffm » Fri Apr 24, 2015 6:45 pm

I have now uploaded the code:

https://github.com/jeffmer/micropython-upybbot

In answer to some of the questions:

The MPU6050 code is a more or less a straighforward use of turbinenreiter's MPU-9150 code.

The approach I used was heavily influenced by Shane Colton's blog: http://scolton.blogspot.co.uk/p/self-ba ... hings.html and by B_ROBOT https://github.com/JJulio/b-robot.

So I use a complementary filter to combine accelerator and gyro - I tried a Kalman, however, I could not get it to reject the mechanical noise generated by the stepper motors. Stability is a PD controller following Shane Colton's white paper and the target angle is set by a simple proportional (P) speed controller. The stability controller takes as input the target angle and outputs an acceleration value which is integrated to give the target speed.

Before discovering Shane Colton's paper I ended up with similar behaviour to that described by pythoncoder. Using the gyro angular velocity directly as the derivative negative feedback worked much better for me than the standard PID approach you see in most web examples. B_ROBOT uses the signal processed quaternions that the mpu6050 can produced, however this requires a complex driver.

Hope this helps.

User avatar
pythoncoder
Posts: 5956
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: Self balancing robot

Post by pythoncoder » Sat Apr 25, 2015 12:52 pm

I'm impressed, not least by the minimal nature of the code, and feel inspired to start from scratch with my effort. Perhaps you could let me know if I'm following the algorithm correctly. Consider the simple case where the robot is balancing around its initial position, i.e. the radio demands zero speed. The line

Code: Select all

fspeed = 0.95 * fspeed + 0.05 * actualspeed
appears to integrate the actual speed to yield a value which presumably corresponds to the distance travelled by the wheels relative to the initial starting point. The code then compares this with the angle as returned by the complementary filter. For small values of angle the angle is proportional to the displacement of the top of the robot relative to its wheels. So if I'm following correctly you're subtracting two distances to yield a distance error - the distance between the top of the robot and its starting point?

Aside from the effect of the derivative rate term, the distance error then gets integrated by the line

Code: Select all

controlspeed += stability(tangle, gangle, rate)           
so the acceleration of the robot base is proportional to the displacement of the top of the robot relative to the robot's start position. Is this broadly correct?

I'd be interested to know how long it took you to optimise the various constants. I'm also curious as to how robust the algorithm is: if you alter the loop period away from 5mS, is there a significant effect on stability? Lastly why do you integrate motor speed to get distance rather than counting the pulses issued to the stepper motors?
Peter Hinch
Index to my micropython libraries.

jeffm
Posts: 23
Joined: Tue Mar 10, 2015 9:11 am

Re: Self balancing robot

Post by jeffm » Sat Apr 25, 2015 1:18 pm

Thanks for your comments, I ditched a lot of irrelevant code once I found Shane Colton's blog. In answer to the questions.

Firstly, the effect of the fspeed line is essentially to act as a low pass digital filter to smooth out fluctuations in speed caused by the stability controller. If the bot is stationary then fspeed will remain at zero.

In answer to your question on stability, I think it is better to think of maintaining the vertical inclination angle at zero. If gravity acceleration causes this to increase as the bot falls from the vertical you need a compensating lateral acceleration from the motors. So the stability control outputs acceleration which is integrated to give speed. In fact, my software makes no attempt to maintain position, however, the speed control means that it will remain stationary after being disturbed by for example a push.

Lastly, the robot still balances with a 10ms loop, however the control is a little coarser and there was a little more vibration. I will do it again now that I have robust balance and let you know. The main challenge is getting the right gain constants for K, Kp and Kd. The standard approach which worked for me is to increase K and Kp until the bot starts to oscillate and then increase Kd until it stops.

Good luck with the reimplementation.

Jonas
Posts: 29
Joined: Fri Jan 23, 2015 8:32 pm

Re: Self balancing robot

Post by Jonas » Sat Apr 25, 2015 2:49 pm

This is great!

I never got the complementary filter to work when I tried balancing the mpu6050 on a servo. I can now see that I was way off with my simple code...
I will now try again.
Thank you!

Post Reply