Inverted Pendulum robot won't balance! (MPU-6050, STM32, PID etc)

[MattHollands]

Hi!

I'm trying to make an inverted pendulum robot - photo attached. Rather than being the usual two-wheeled robot, it is actually just a single motor rotating in the vertical axis while the pivoting arm rotates in the horizontal axis (just look at the picture ). I also have some video (filmed at 4x slow) showing it in action - it has definitely performed better in the past. https://www.youtube.com/watch?v=80mq2CQVi0U&feature=youtu.be

But I just can't get it to balance. Generally it is quite jittery and has a tendency to oscillate/slam itself into the floor. I've tried for hours and hours over multiple weeks to get the PID loop tuned, but I just can't get something that works. I'm really not sure if it is something to do with the mechanics or the code.

I am using an MPU-6050 gyro/acc chip hooked up to an STM32 which controls a MP6513 motor controller. The motors are controlled by PWM. The loop time is approximately 1ms.

Since this is a pretty popular project idea with a fairly common gyro/acc IC I was hoping someone could point me in the right direction. My complete code can be found at https://github.com/mhollands/SpinBalance/blob/master/STM32_FW/Src/main.c but I will summarize here.

Register settings
This is run once before the main loop

Code: [Select]

MPU_SetReg(0x6b, 0x00); //Set power mode
  MPU_SetReg(0x1c, 0x10); // Set accelerometers to +/-4g
  MPU_SetReg(0x1b, 0x10); //Set gyros to +/-500deg/sec

Reading accelerometer and gyro
Read the 6 acc/gyro values and convert to grams and deg/sec.

Code: [Select]

// Read Acceleration values
	  MPU_GetReg(0x3b, acc_regs, 6);
	  acc_vals[0] = ((int16_t)(acc_regs[0] << 8) + acc_regs[1])/8192.0; //Convert force to grams
	  acc_vals[1] = ((int16_t)(acc_regs[2] << 8) + acc_regs[3])/8192.0; //Convert force to grams
	  acc_vals[2] = ((int16_t)(acc_regs[4] << 8) + acc_regs[5])/8192.0; //Convert force to grams

	  // Read Gyro values and convert to
	  MPU_GetReg(0x43, gyro_regs, 6);
	  gyro_vals[0] = ((int16_t)(gyro_regs[0] << 8) + gyro_regs[1])/65.5 + 0.1620; //Convert to degs/s
	  gyro_vals[1] = ((int16_t)(gyro_regs[2] << 8) + gyro_regs[3])/65.5; //Convert to degs/s
	  gyro_vals[2] = ((int16_t)(gyro_regs[4] << 8) + gyro_regs[5])/65.5 - 1.1653; //Convert to degs/s

Calculate angle
Final angle is a combination of the data from accelerometer and the gyro.

Code: [Select]

// Calculate the accelerometer angle
	  float accAngle = (atan2(acc_vals[0],acc_vals[1]) * 180 / PI);

	  // Get time delta
	  uint32_t t_now = HAL_GetTick();
	  uint32_t t_delta = t_now - t;
	  delay_time += (TARGET_PERIOD - t_delta);
	  t = t_now;

	  // Combine accelerometer and gyro angles
	  angle = 0.98*(angle + gyro_vals[2]*t_delta/1000) + 0.02*accAngle;

Calculate PID
Using values for Ki, Kp, Kd, integrate error etc.

Code: [Select]

balance(angle+1, gyro_vals[2], t_delta);

Code: [Select]

i_error += angle * t_delta / 1000;
	int16_t desired_speed = (int16_t)(i_error*ki + angle*kp + delta_angle*kd);
	current_speed = desired_speed;
	SET_PWM_SPEED(current_speed);

Are there any rookie errors I should be looking out for?
Are there any simple tests I could do to root cause the issue?
Any debugging tips.

[SiliconWizard]

I haven't looked at it in great details, but first thing I would question is your final estimation of the pendulum angle. Are you sure it's correct?

[ebastler]

In your video, I don't actually see oscillations. Rather it looks like the device detects that the pendulum is tilting, and tries to move the support in the same direction to get it back underneath the center of gravity -- but doesn't do that fast enough, so the pendulum leans over further and further, while the support follows behind.

Do you have your parameters tuned correctly? And are you sure the motor can physically accelerate fast enough to keep up with the tipping pendulum? (The pendulum is rather short, and hence tips over with a pretty high angular velocity.)

[ogden]

The pendulum is rather short, and hence tips over with a pretty high angular velocity.

Exactly. First fall of pendulum shows that motor do not keep-up with fall. I would immediately try to lengthen 3x times or so (move mass to the very end obviously), see how it goes then.

[mstevens]

What does your modelling and math say?

[Mr. Scram]

What does your modelling and math say?

That you assume a lot.

[mstevens]

What does your modelling and math say?

That you assume a lot.

   Yeah, that is evident... just trying to get him to see it 

[MattHollands]

I haven't looked at it in great details, but first thing I would question is your final estimation of the pendulum angle. Are you sure it's correct?

What I have done is have the STM32 report it's measured angle over USB and it looks pretty correct. However, I can only check this in the static case because it needs the USB plugged in. While the robot is moving I need to remove the USB cable which means I cannot easily track the calculated angle in a dynamic state.

In your video, I don't actually see oscillations. Rather it looks like the device detects that the pendulum is tilting, and tries to move the support in the same direction to get it back underneath the center of gravity -- but doesn't do that fast enough, so the pendulum leans over further and further, while the support follows behind.

Do you have your parameters tuned correctly? And are you sure the motor can physically accelerate fast enough to keep up with the tipping pendulum? (The pendulum is rather short, and hence tips over with a pretty high angular velocity.)

The pendulum is rather short, and hence tips over with a pretty high angular velocity.

Exactly. First fall of pendulum shows that motor do not keep-up with fall. I would immediately try to lengthen 3x times or so (move mass to the very end obviously), see how it goes then.

Fair point. I guess what I'm referring to is when I hold the robot still it jerks around a lot in an oscillating kind of way - it doesn't oscillate when released. I will increase the pendulum length and see if that improves things. Will report back.

  Yeah, that is evident... just trying to get him to see it 

See what? That modeling and maths exist? Thanks, genius.

[mstevens]

  Yeah, that is evident... just trying to get him to see it 

See what? That modeling and maths exist? Thanks, genius.
[/quote]

No, that you need to look and the system mathematically to figure out your problem, or at least that is the fastest way to solve it; see control theory...

[Mr. Scram]

  Yeah, that is evident... just trying to get him to see it 

See what? That modeling and maths exist? Thanks, genius.

No, that you need to look and the system mathematically to figure out your problem, or at least that is the fastest way to solve it; see control theory...

Maybe a few pointers how you'd go about this would help. Tuning PID systems isn't quite an exact science and can be a dark art.

[ogden]

PID tuning may seem like art or magic, but usually it is not - *when* you understand basic principles. My usual "go to" article I suggest to "PID beginners": https://www.wescottdesign.com/articles/pid/pidWithoutAPhd.pdf. Obviously there are more digital control articles & books around.

[SiliconWizard]

I haven't looked at it in great details, but first thing I would question is your final estimation of the pendulum angle. Are you sure it's correct?

What I have done is have the STM32 report it's measured angle over USB and it looks pretty correct. However, I can only check this in the static case because it needs the USB plugged in. While the robot is moving I need to remove the USB cable which means I cannot easily track the calculated angle in a dynamic state.

OK. Well, it would be interesting to see how the calculated angle turns out once the thing is moving. When it's not, all dynamic accelerations will be zero, the only thing you'll get are the static acceleration (gravity basically)... in which case it's not surprising that the result is correct. I'm not saying there's a fault in what you did, but that's just what I would suspect first. (Method! Check your inputs first... )

[Mr. Scram]

PID tuning may seem like art or magic, but usually it is not - *when* you understand basic principles. My usual "go to" article I suggest to "PID beginners": https://www.wescottdesign.com/articles/pid/pidWithoutAPhd.pdf. Obviously there are more digital control articles & books around.

Various tuning methods existing but I don't think I've ever seen one work without some practical experimenting and they tend to tune the system quite aggressively. Knowing the basic principles helps but that's where some of the art comes in. It seems rare to boot a system up and have it work first go.

[ogden]

PID tuning may seem like art or magic, but usually it is not - *when* you understand basic principles. My usual "go to" article I suggest to "PID beginners": https://www.wescottdesign.com/articles/pid/pidWithoutAPhd.pdf. Obviously there are more digital control articles & books around.

Various tuning methods existing but I don't think I've ever seen one work without some practical experimenting and they tend to tune the system quite aggressively. Knowing the basic principles helps but that's where some of the art comes in. It seems rare to boot a system up and have it work first go.

By practical experimenting you mean tuning PID variables using trial and error approach, other name for it is "PID tuning magic"? Yes, why not - as long as it works for you. Thou there are other ways. Further reading: https://www.mathworks.com/company/newsletters/articles/tuning-a-pid-controller-when-a-plant-model-is-not-available.html

[mikerj]

The very sluggish response suggests the PID gains are way too low.  Typically these simple PID based balancing systems tend to have gains high enough that they are slightly jittery around the balance point.

To manually tune a system I usually start with the Ziegler-Nichols tuning method to get proportional gain in the ball park, i.e. start with P only gain (I and D set to zero) and wind it up until the system starts oscillating, then reduce it to about 0.6 of this value.  The actual Ziegler-Nichols derives the other gains from period of oscillation, but one I have P somewhere close I usually just add integral gain until the step response is reasonable then tweak all three gains.  Once you get a feel for the impact that each term has on the response you can home in on suitable values fairly quickly.

If it's not currently included I'd advise adding some method of logging the outputs from your sensors (e.g. serial link) so change to the control loop performance from modifying PID gains can be easily visualised in e.g. a spreadsheet. 

[thinkfat]

How many computation loops do you get per second? Is your MCU actually fast enough for all that floating point stuff you're doing?

[Mr. Scram]

By practical experimenting you mean tuning PID variables using trial and error approach, other name for it is "PID tuning magic"? Yes, why not - as long as it works for you. Thou there are other ways. Further reading: https://www.mathworks.com/company/newsletters/articles/tuning-a-pid-controller-when-a-plant-model-is-not-available.html

You seem to be misconstruing what I said and also adding to it. I said PID tuning isn't an exact art. Dicking around until you get something serviceable would justify that qualification. Having some idea of control theory just ensures your making educated guesses instead of blind stabs. Having a feel for where to guess and stab is wherein the art lies. The dark part occasionally surfaces when you have dicky systems that just won't behave.

[ogden]

Having some idea of control theory just ensures your making educated guesses instead of blind stabs. Having a feel for where to guess and stab is wherein the art lies. The dark part occasionally surfaces when you have dicky systems that just won't behave.

Not every control task is hobby drone. Sometimes you have to work out plant model and tune PID w/o educated guesses unless you want to waste months of your time and/or damage expensive stuff/materials. Simple example: boiler which cools down during few days and can't go above certain temperature no matter what (otherwise contents are damaged).

[Mr. Scram]

Not every control task is hobby drone. Sometimes you have to work out plant model and tune PID w/o educated guesses unless you want to waste months of your time and/or damage expensive stuff/materials. Simple example: boiler which cools down during few days and can't go above certain temperature no matter what (otherwise contents are damaged).

You mean there's some art in using the appropriate models or approach as it depends on the situation and isn't quite a clear cut problem with an equally clear cut path to a solution? That sounds about right. Sounds familiar too.

[MattHollands]

Ok I've made the pendulum arm a bit longer and it is performing better: https://youtu.be/KH-BAVT16ps
I have printed some "arm extensions" but unfortunately I had to bolt them together with metal bolts so the weight is not really at the top. I have ordered some plastic bolts so I can move all the weight to the top and see if that improves things further.

Thanks for all the tips on tuning - I seem to be getting closer.

How many computation loops do you get per second? Is your MCU actually fast enough for all that floating point stuff you're doing?

My loop time is about 1ms.

If it's not currently included I'd advise adding some method of logging the outputs from your sensors (e.g. serial link) so change to the control loop performance from modifying PID gains can be easily visualised in e.g. a spreadsheet. 

Lol yep totally wish I had included a radio chip or something so I could stream data off it. If I spin a second board I may well do this.

[mstevens]

Did you ever get this project working reliably?