Inverted Pendulum robot won't balance! (MPU-6050, STM32, PID etc) - Page 1
Forum section: electronics / projects

author: MattHollands
author_rank: Frequent Contributor
timestamp: January 05, 2020, 04:40:31 pm
content: 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.beBut 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 athttps://github.com/mhollands/SpinBalance/blob/master/STM32_FW/Src/main.cbut I will summarize here.Register settingsThis is run once before the main loopCode:MPU_SetReg(0x6b, 0x00); //Set power modeMPU_SetReg(0x1c, 0x10); // Set accelerometers to +/-4gMPU_SetReg(0x1b, 0x10); //Set gyros to +/-500deg/secReading accelerometer and gyroRead the 6 acc/gyro values and convert to grams and deg/sec.Code:// Read Acceleration valuesMPU_GetReg(0x3b, acc_regs, 6);acc_vals = ((int16_t)(acc_regs << 8) + acc_regs)/8192.0; //Convert force to gramsacc_vals = ((int16_t)(acc_regs << 8) + acc_regs)/8192.0; //Convert force to gramsacc_vals = ((int16_t)(acc_regs << 8) + acc_regs)/8192.0; //Convert force to grams// Read Gyro values and convert toMPU_GetReg(0x43, gyro_regs, 6);gyro_vals = ((int16_t)(gyro_regs << 8) + gyro_regs)/65.5 + 0.1620; //Convert to degs/sgyro_vals = ((int16_t)(gyro_regs << 8) + gyro_regs)/65.5; //Convert to degs/sgyro_vals = ((int16_t)(gyro_regs << 8) + gyro_regs)/65.5 - 1.1653; //Convert to degs/sCalculate angleFinal angle is a combination of the data from accelerometer and the gyro.Code:// Calculate the accelerometer anglefloat accAngle = (atan2(acc_vals,acc_vals) * 180 / PI);// Get time deltauint32_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 anglesangle = 0.98*(angle + gyro_vals*t_delta/1000) + 0.02*accAngle;Calculate PIDUsing values for Ki, Kp, Kd, integrate error etc.Code:balance(angle+1, gyro_vals, t_delta);Code: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.

author: SiliconWizard
author_rank: Super Contributor
timestamp: January 05, 2020, 05:15:47 pm
content: 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?

author: ebastler
author_rank: Super Contributor
timestamp: January 05, 2020, 05:19:37 pm
content: 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.)

author: ogden
author_rank: Super Contributor
timestamp: January 05, 2020, 05:35:55 pm
content: Quote from: ebastler on January 05, 2020, 05:19:37 pmExactly. 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.

author: mstevens
author_rank: Regular Contributor
timestamp: January 05, 2020, 05:40:29 pm
content: What does your modelling and math say?

author: Mr. Scram
author_rank: Super Contributor
timestamp: January 05, 2020, 05:48:32 pm
content: Quote from: mstevens on January 05, 2020, 05:40:29 pmThat you assume a lot.

author: mstevens
author_rank: Regular Contributor
timestamp: January 05, 2020, 06:36:35 pm
content: Quote from: Mr. Scram on January 05, 2020, 05:48:32 pmYeah, that is evident... just trying to get him to see it

author: MattHollands
author_rank: Frequent Contributor
timestamp: January 05, 2020, 07:30:16 pm
content: Quote from: SiliconWizard on January 05, 2020, 05:15:47 pmWhat 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.Quote from: ebastler on January 05, 2020, 05:19:37 pmQuote from: ogden on January 05, 2020, 05:35:55 pmFair 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.Quote from: mstevens on January 05, 2020, 06:36:35 pmSee what? That modeling and maths exist? Thanks, genius.

author: mstevens
author_rank: Regular Contributor
timestamp: January 05, 2020, 11:54:13 pm
content: Quote from: mstevens on January 05, 2020, 06:36:35 pmSee 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...

author: Mr. Scram
author_rank: Super Contributor
timestamp: January 06, 2020, 12:07:08 am
content: Quote from: mstevens on January 05, 2020, 11:54:13 pmMaybe 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.

author: ogden
author_rank: Super Contributor
timestamp: January 06, 2020, 12:55:37 am
content: 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.

author: SiliconWizard
author_rank: Super Contributor
timestamp: January 06, 2020, 01:00:01 am
content: Quote from: MattHollands on January 05, 2020, 07:30:16 pmOK. 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...)

author: Mr. Scram
author_rank: Super Contributor
timestamp: January 06, 2020, 06:50:31 am
content: Quote from: ogden on January 06, 2020, 12:55:37 amVarious 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.

author: ogden
author_rank: Super Contributor
timestamp: January 06, 2020, 07:17:10 am
content: Quote from: Mr. Scram on January 06, 2020, 06:50:31 amBy 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

author: mikerj
author_rank: Super Contributor
timestamp: January 06, 2020, 11:12:28 am
content: The very sluggish response suggests the PID gains arewaytoo 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 theZiegler-Nicholstuning 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.

author: thinkfat
author_rank: Unknown
timestamp: January 06, 2020, 12:28:45 pm
content: How many computation loops do you get per second? Is your MCU actually fast enough for all that floating point stuff you're doing?

author: Mr. Scram
author_rank: Super Contributor
timestamp: January 06, 2020, 02:52:34 pm
content: Quote from: ogden on January 06, 2020, 07:17:10 amYou 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.

author: ogden
author_rank: Super Contributor
timestamp: January 06, 2020, 03:08:16 pm
content: Quote from: Mr. Scram on January 06, 2020, 02:52:34 pmNot 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).

author: Mr. Scram
author_rank: Super Contributor
timestamp: January 06, 2020, 06:09:56 pm
content: Quote from: ogden on January 06, 2020, 03:08:16 pmYou 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.

author: MattHollands
author_rank: Frequent Contributor
timestamp: January 13, 2020, 04:33:50 am
content: Ok I've made the pendulum arm a bit longer and it is performing better:https://youtu.be/KH-BAVT16psI 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.Quote from: thinkfat on January 06, 2020, 12:28:45 pmMy loop time is about 1ms.Quote from: mikerj on January 06, 2020, 11:12:28 amLol 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.

author: mstevens
author_rank: Regular Contributor
timestamp: January 17, 2020, 02:33:16 pm
content: Did you ever get this project working reliably?