Self balancing robot

[pythoncoder]

@Jeffm Thank you for your helpful comments. The fact that doubling the loop delay doesn't seriously affect stability is encouraging, along with the fact that it works so well without Kalman filtering or Quaternians (surely completely OTT for this purpose). From my own experiments and reading I was becoming convinced that a complementary filter was adequate but it's good to have that confirmed.

I'm impressed that your bot holds position without explicit feedback which further confirms the robustness of your solution. I'd assumed from the outset that position feedback would be essential so mine has optical encoders. It uses DC motors so speed and drive voltage are linearly related - my encoders would seem to be redundant. I'll base my solution as closely as possible on yours and see how that progresses.

One more question about the algorithm. Why do you convert to integers in the output of stability() and the input to speedcontrol()?

In desperation I did consider a different approach to control. The starting point would be a control system which would drive the robot to a specific position. With the bot stationary you read the filtered angle and, from the gyro, its first derivative. From those calculate the position to which the base would have to be moved so that, at a future time, the angle and its first derivative would reach zero. Then move to that position and await the outcome. This could be repeated whenever the angle error exceeded a threshold; I've not heard of anyone using this approach. It would lead to rather jerky motion and I much prefer a linear solution but I remain curious as to whether it could be made to work.

I'll let you know how I get on with adapting your code

[jeffm]

@pythoncoder. One more question about the algorithm. Why do you convert to integers in the output of stability() and the input to speedcontrol()?

I measure speed in micro steps per second, so the stepper class uses integers and in particular it is important that the interrupt service routine uses integers for efficiency so the output from the stability control is integer. As the speed control does floating point operations and there is no point in the integer conversion on input - it's a legacy conversion which I will remove . Thanks for pointing this out.

BTW: I think you may need your optical encoders to get an accurate speed measurement as there may be lag between setting the pwm or voltage value and the motor accelerating to the new set speed.

One point that you are probably already aware of is that it is good to have some weight at the top of the robot as the inertia means that you need slower response times for control - think of balancing an umbrella on your fingertip versus a pencil. I have 10 AA Nicad batteries at the top and the whole bot weighs in at 1kg.

Lastly, I think you would be getting into deep water with the approach you were thinking about in the last paragraph as it is proposing a form of model based control in which you would need a full mathematical description of the inverted pendulum problem.

I would be very interested to hear how you get on.

[pythoncoder]

BTW: I think you may need your optical encoders to get an accurate speed measurement as there may be lag between setting the pwm or voltage value and the motor accelerating to the new set speed.

It would be more accurate but would introduce a phase lag owing to the effect of the inertia of the motor shaft and the robot base. I've tried to minimise the lag in the design of my bot: the wheel motors are geared down 120:1 and the base of the robot is light so the motors accelerate to full speed very quickly. My concern with reading actual speed is that introducing an extra phase lag into the feedback loop might make it harder to stabilise. I think I'll work on demand speed to begin with.

it is good to have some weight at the top of the robot

Indeed. My bot has a steel mass at the top of a 1 metre long support: at the outset I wanted to make the time constant as long as practical as I figured it would make the control system more tolerant of delays in the Python code. The evidence of your success indicates that I was being over cautious. This robot was always intended as a proof of concept (of using MicroPython) prior to building a new chassis without the tether and with battery and radio control.

I have now got your code running on my robot (with minor adaptations for the DC motors). Today I will start trying to stabilise the thing, I'll let you know if I have any joy.

I entirely take your point about the mathematical model, and judging by some of the papers I've read on the subject it is, to coin a phrase, "nontrivial". I merely wondered whether the approach had ever been tried.

[pythoncoder]

@jeffm A remark you made in our discussion about filtering has made me realise why I may have been having so much trouble with this: you mentioned vibration. To date my testing of the IMU and the filters I've tried was done without the motors running. I've now tried logging the data read from the IMU with the motors running but the robot stationary and the levels of vibration are severe: I'm getting readings of up to 0.5g. My testing suggests that relying on code to filter this out is problematic and I need some mechanical isolation. Did you do anything to mechanically isolate the MPU9160 from the robot assembly? Have you (or has anyone else) any bright ideas of how to do this?

Incidentally while doing this it struck me that, for the range of angles where your code is in the linear region, the following pieces of code produce results which are close approximations:

Code: Select all

def pitchtest(x, z):
        pitch = degrees(pi+atan2(-x,-z))
        if (pitch>=180) and (pitch<=360):
            pitch-=360
        return -pitch

Code: Select all

degrees(-x)

This is because z is approximately unity (1G) and atan(phi) ~= phi for small angles.

[jeffm]

@pythoncoder I tried using a piece of foam to isolate the imu, however, it made things worse:-( I found that setting the complementary filter to have a response time of 0.5secs worked as it rejected the rapid gravity fluctuations due to mechanical noise. The gyro is not affected so it is sufficient to correct the drift using this 0.5 second low pass filtering.

Your simplification is also suggested by Shane Colton and is really valuable if cpu time is limited. Since the pyboard has a floating point processor I did not worry and I wanted the angular display to be accurate even for large angles.

[pythoncoder]

I'm evidently experiencing a higher level of vibration than you. A more rigorous check indicates levels of +-1g which astonished me and was overwhelming the complementary filter. The MPU9150 has a built in digital filter and I've adapted @turbinereiter's library to enable it to be configured. It makes a big difference, with the 5Hz setting reducing my levels to +-0.09g. However this might affect stability by introducing a phase lag. I'll attempt mechanical isolation in the hope of being able to use a higher cutoff frequency: apparently aero modellers do this routinely.

Vibration is clearly a potential issue with these things. With a control loop running at 200Hz, any vibration above 100Hz is liable to be aliased down and appear as a spurious low frequency. I think a combination of mechanical isolation and the built-in filtering may be my way forward unless I abandon my DC motors which are the cause of the problem. Another approach would be to isolate the motor/wheel assemblies from the chassis.

As for the code I'll submit a PR to @turbinereiter in due course. I can post it here if you or anyone else is interested, but I guess you have no need as your robot works

I take your point about atan2() which I posted purely as a point of interest. If it ain't broke, don't fix it

[jeffm]

I would really interested in your code to access the digital filtering code so please do post it. I reduced the accelerometer sensitivity from + or - 2g to + or - 4g which helped a bit, however, I think I still have some residual unnecessary vibration when stationary.

[pythoncoder]

I've posted the code here (and submitted a PR to @turbinenreiter) https://github.com/peterhinch/micropython-mpu9150.git. The README is also updated with some detail from the device manual. The relevant method is

Code: Select all

    def filter_range(self, filt=None):
        '''
        Returns the gyro and accel low pass filter cutoff frequency
        Pass:               0   1   2   3   4   5   6   7
        Cutoff (Hz):        260 184 94  44  24  10  5   n/a
        '''
        # set range
        try:
            if filt is None:
                pass
            else:
                if (filt >= 0) and (filt < 7):
                    self._write(filt, 0x1A, self.mpu_addr)
                else:
                    print('Filter coefficient must be between 0 and 6')
            # get range
            res = self._read(1, 0x1A, self.mpu_addr)[0] & 7
        except OSError:
            res = None

        return res

I'd be very interested to hear what value you settle on, and whether the lower cutoff frequencies affect the robot stability.

[Turbinenreiter]

Is now merged in the MPU9150 module.

[Jonas]

Hehe...This is my "self balancing robot", it can't balance and its not much of a robot...

I have alot of noice, and I can't figure out how to get the motors to run smoothly. Even with the mpu turned off the motors vibrates alot.
This is the same motor drivers and motors I use for my 3D-printer, and that run without any problems
Well I guess I have to spend alot more time on it, can't expect it to work the first time...

balans2 (Medium).jpg (105.33 KiB) Viewed 6380 times

My setup:
mpu: MPU 9150
Driver: md09c/A4988
steppers: nema 17, 400 step/rev
remote: RX3302D