Self balancing robot

[jeffm]

@pythoncoder I am afraid that the filter did not help much as when I got to cutoff frequencies - range 5, 10Hz - that helped smooth out vibration, the robot became unstable due to the additional delay, even when I increased negative feedback. Predicable I suppose. The problem is that the filter delays both accelerometer and gyro while the complementary filter only delays the accelerometer. I have come to the conclusion that the vibration I get when the robot is stationary is primarily the robot maintaining balance. Next step to damping this will be to try 8 or 16 micro steps per step rather than the current 4.

@jonas good luck with getting it to balance. I noticed from the photo that it looks like you have positioned the 9150 board at the top of the robot. Most web articles advise locating this as close as possible to the axis of rotation - usually between the motors - to reduce noise on the accelerometer. In my case, it's on the shelf above the motors, so it's not that critical.

[Jonas]

Ah! Thanks! I just assumed that it would pick up the tilt faster if it sits on the top...
I got plenty of room just above the motors.
But my main problem seems to be that the A4988 drivers pick up alot of noice. Just by placing my hand near the Step, Dir and En wires it starts to jerk alot more. I tried to change the drivers to two new ones, but still the same. I did not have this problem with my 3D-printer.
I can't see how the pyboard would have anything to do with it, except that it's 3.3 volt logic and the 3D-printer is 5 volt logic.
I just can't figure it out

[pythoncoder]

@jeffm Thanks for the information. I can't say I'm surprised. I'll concentrate on mechanical isolation.

As for the sensor position, the angle to the vertical will be the same wherever it is measured. This implies that the rate measured by the gyro will also be identical. What varies is the response of the accelerometer to the motion of the wheels. Say the wheels move the axle forwards by a small distance, the part of the robot which pivots around the axle will rotate about its centre of mass. The torque causing this rotation arises because the centre of mass will, in the absence of a force to accelerate it, remain stationary. It will then begin a gradual acceleration caused by gravity - which would cause the robot to topple if it weren't for the control system.

In the long term, because of the action of the control loop, the centre of mass will move at the same speed as the axle. The control system manipulates the angle and gravity provides the acceleration forces to achieve this. In the long term at the centre of mass the acceleration, integrated over time, must match the integral of that experienced by the axle (otherwise they'd move at different speeds). However the short term high frequency accelerations will be smaller than those of the axle.

This would seem to create a case for mounting the sensor near the centre of mass. However perhaps the complementary (or Kalman) filter is so good at removing these higher frequencies that there's no practical benefit. Or perhaps I'm missing something here.

[pythoncoder]

To anyone trying to make a robot balance, watch these and weep. Especially the first short clip.
https://www.youtube.com/watch?v=cyN-CRNrb3E
https://www.youtube.com/watch?v=w2itwFJCgFQ Quadcopter athletics, including a broom balance.

[blmorris]

@pythoncoder - Wow! As for the triple pendulum - they are using a single control input (x-position of the cart) to balance a 3-DOF system in an unstable configuration. There must be some trick about the degrees of freedom not being completely independent, but I'm quite certain they never covered that trick in any control-systems class I ever took. Even seeing the video, I haven't quite convinced myself that it is actually possible… time for me to look up their research paper.

-Bryan

[pythoncoder]

@blmorris According to the guy who pointed me at the video and who's an engineering maths guru - there is maths that proves that balancing a triple inverted pendulum is possible subject to certain rod length limitations. But it doesn't tell you how actually to achieve it

@jeffm I've now rebuilt my machine on a new chassis which includes mechanical vibration isolation. This uses a steel mass of 26g with very soft rubber mountings and I've used very thin, relatively long, wires to avoid transmitting vibration down them. The picture shows the robot base with the superstructure removed, shown in case you want to go down a similar route.

The outcome is that vibration as measured by the accelerometer is down from +-1g to +=0.25g. But the output of the complementary filter is vastly better and is of the order of +-1 degree. This is using a filter setting on the MPU-9150 of 2 (3mS delay), and with the robot base supported with the wheels running at full speed. The complementary filter will tend to remove short term jitter on the accelerometer but pass it on the gyro. I think the mechanical isolation must have greatly reduced high frequency vibration signals from the gyro. Alas I never measured that parameter on the old rig but the output of the filter was atrocious. So I'm very happy with the result.

Over the last few months I've read a lot of accounts of builds and as far as I can remember none mentioned vibration as a serious issue and it never occurred to me to measure it. Thanks very much indeed for the pointer.

Now to bolt the top on and try to get the thing to balance...

[pythoncoder]

@jeffm Success! After the rebuild I soon had it balancing, and it maintains position for long periods which I find remarkable given the fact that I'm not using explicit position feedback. Evidently vibration had been the bugbear from the start, although my original approach to coding was also wrong. I'd failed to appreciate how quickly the control loop has to operate to achieve stability although realisation was dawning at the time I shelved it.

On the topic of vibration isolation, before final reassembly I did a more precise test of the filtering. With the robot base supported and the wheels running at full speed I analysed the output of the complementary filter over 200 cycles of the control loop (with the IMU filtering set to 2). The results were consistent with errors of around 0.08 degrees RMS each time. So mechanical isolation can be highly effective.

Thanks again for the pointer on vibration. I may get back with some other comments once I have a more complete solution in the form of a robot which does more than stand still...

[jeffm]

@pythoncoder. Congratulations! Really pleased you got it to balance. Thanks for the information on mechanical vibration damping, I am looking at how to implement it on my chassis. I am working on more sophisticated control using an ESP8266 WiFi board and a mobile phone which I will share when I get it a little further along.

[pythoncoder]

I've made a few changes to the algorithm which I thought I'd share to see what you think. These are intended as food for thought
and aren't intended as criticism. First a couple of minor points on program style.

The local gangle and global gangle variables are identical. To avoid this and to lose an unneccesary global, I have a local fangle
variable set to zero at the start of the main balance loop. The CF now looks like this

Code: Select all

def compf(fangle, accel,gyro,looptime,A):
    return A * (fangle + gyro * looptime/1000000) + (1-A) * accel

Secondly a neat way to implement a symmetrical constraint is

Code: Select all

def constrain(val, maxv):
    return min(maxv, max(-maxv, val))

As for the algorithm I wanted to simplify this already simple function further to help me get my elderly brain round how it works. In particular minimising the number of global "magic numbers" and processing blocks. The system has one inevitably nonlinear element which is the motor speed. This will be constrained by physics if not in code It seemed to me that the additional constraint on the angle could be eliminated. The input demand speed must already be constrained outside the loop to be less than the motor's capability otherwise the 'bot will fall over . This, with the fact that the output speed is constrained sets a limit on the error signal. Hence, with a suitably chosen value of KpS, the demand angle is also constrained. I therefore choose KpS to ensure that the output of speedcontrol() is within +=7 degrees and eliminate the explicit constraint:

Code: Select all

# Motor max is 377mm/s, max demand 300mm/s so max error 677mm/s
KpS = 0.01  # +-6.77 degrees
def speedcontrol(target, current):
    return KpS*(target - current)

KpS obviously affects loop gain which can be compensated for by the choice of the value of K. I've modified the stability routine to reduce the number of constants to two: K and Kd - this is just algebra, but K sets the proportional gain in conjunction with the previously chosen KpS. Kd sets the derivative. In opposition to my desire to reduce global constants I've added one, LOOPTIME. This is the time to complete a single loop (currently 5000uS). This is because I wanted to experiment with different delay values and at present the integrator and the fspeed filter are dependent on this. With an explicit value the loop parameters can be made independent of duration. So I now have:

Code: Select all

K = 5.0
Kd = 0.7
LOOPTIME = const(3000)                            # uS per loop
def stability(target,current,rate):
    error = target - current
    return K * (error - Kd*rate) * LOOPTIME/5000  # 5000 is nominal loop time

Obviously LOOPTIME can be eliminated once a value has been chosen, and my current view is that your existing 5000 is good.

On the subject of the first order low pass filter on fspeed I'm using the computed value of motor speed as a proxy for the actual speed: with lightly
loaded DC motors it's pretty accurate in the steady state. Measuring the actual speed is an option, but would introduce additional phase lags. I thought, at least initially, I'd avoid this in the interests of simplicity. So the question arose as to whether there is any point in filtering a computed value coming from an integrator. In practice I found the filter seemed to make the robot harder to stabilise, presumably because of the extra pole it introduces. So I removed it.

My main loop now looks like this - with the speed and turn control removed for clarity. Your code for movement and rotation works fine on my 'bot.

Code: Select all

def balance():
    start = pyb.micros()
    controlspeed = 0
    fspeed = 0
    demandspeed = 0
    fangle = 0.0
    while abs(fangle) < 45:  # give up if inclination angle >=15 degrees
        angle  = imu.pitch()
        rate   = imu.get_gy()
        fangle = compf(fangle, angle, rate, pyb.elapsed_micros(start)) # filtered angle
        start = pyb.micros()
        fspeed = (motor1.speed + motor2.speed)/2
        tangle  = speedcontrol(demandspeed, fspeed)      # Get target angle
        controlspeed += stability(tangle, fangle, rate)  # stability control
        motor1.speed = -controlspeed                     # set motor speed in mm/s
        motor2.speed = -controlspeed
        pyb.udelay(LOOPTIME - pyb.elapsed_micros(start))

    motor1.speed = 0                                     # stop and turn off motors
    motor2.speed = 0

motor.speed returns the demand speed constrained to +-377 (units are mm/s at the wheel rim)

As for performance, the code runs in about 550uS so pyb.udelay() is passed around 4.4mS every 5mS loop. As you suggested, this is regardless of whether the atan2() simplification is used. So I use atan2(). The Pyboard clearly offers scope for faster looping but benefits are minimal. I tried values from 2mS to 20mS: the bot continued to balance but positional stability started to degrade for longer loop times.

On a final note - and this is a big one - the thought occurred to me that ideally the loop would be executed as a callback on a timer running
at 5KHz. This would allow the Pyboard to do concurrent processing such as handling radio control or sensors, passing demand speed to the handler running in the background. Challenging, because callbacks can't use the heap. Thus we'd either need to do integer processing with its scaling issues or use the inline assembler for floating point. The MPU-9150 driver would need some adaptation as by default its get_gyro() and get_accel() methods use the heap. And there is the question of whether the underlying I2C routines use it. I'll investigate in due course.

<later> Some quick tests indicate that running as a timer callback is a non-starter. The MicroPython I2C driver uses the heap.

[pythoncoder]

In case anyone's still following this thread here is my effort so far. Still to do (hence the rats' nest wiring up the tower):

• * Build in the charger for the sealed lead acid battery.

• * Replace the cheapo radio with an NRF24L01

• * Build the new radio controller to suit (with another Pyboard and NRF24L01)