Go Down

Topic: Inverted Pendulum Cart Position Control PID Help (Read 189 times) previous topic - next topic

kRz_4tw

Jul 06, 2016, 11:10 pm Last Edit: Jul 07, 2016, 04:41 am by kRz_4tw
Hey guys,

i´ve been building an inverted pendulum out of an old printer. Im using Quadrature Encoders for the angle of pendulum and the linear position of the cart.

Right now im at the point where I got the system set up so that it will balance the pendulum using the PID library. The PID Controller is solely looking at the angle of the pendulum and does not use the linear position signal in any way. I haven´t implemented an automated swing up and dont´t intend to do so far.

I´d now like to find a way to balance the pendulum at a certain linear position along the rail.

Besides it would be just cool to do that I actually need that feature for stable system. What happens at the moment is that the pendulum is stabilized upright but the cart is still moving eventually hitting the end of travel. The cart will move with a constant speed everlasting trying to catch up with the pendulum that still has some residue velocity at the top left and try´s to tip over therefore.

Because it´s just a desktop printer i run out of travel very fast so though the pendulum is stabilized it will only work for about two seconds before in runs into the end of travel.

I need some way to control the carts position along with the angle. That seems to be tricky because I need control two variablses with just one input(motor) into the system. Furthermore the linear control would basically need overshoot first to get the cart to reverse its direction. That is somewhat of the contrary of what the "angle"-controller is trying to do.


Can anyone give me some clues how to realize a linear control of the system?
Iv´e been googeling like crazy but can only find academic papers that are just beyond me.



Thanks in advance guys.

Kevin




The Code for anyone whos interested.
It´s not particularly pretty I must admit.

Code: [Select]

#include <PID_v1.h>
#include <Encoder.h>



//max value for linear encoder until it hits physical stop       Encoder count =6716
byte pin_photoint=13;
byte pin_pwm=6;
byte pin_left=7; //Left and right might change depending how you connected the dc motor. CHECK THIS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
byte pin_right=8;
byte pin_lin_enc1=2;
byte pin_lin_enc2=3;
byte pin_rot_enc1=18;
byte pin_rot_enc2=19;
byte setup_speed=105;
byte rot_deadband=1; //In counts in each direction

int limit_lin_min=500;    //min Limit (in direction torwards limit switch--> right)
int limit_lin_max=6200;   //max Limit (left)
double limit_angle_left=4000;
double limit_angle_right=3200;
double angle;
double lin_position;



Encoder linEnc(pin_lin_enc1,pin_lin_enc2);  //Setup of linear Encoder, overall length about 6700 counts              
Encoder rotEnc(pin_rot_enc1,pin_rot_enc2);  //Setup dof rotary Encoder, one revolution=7200 counts

//PID Variables

double Output, rot_Setpoint, lin_Setpoint;
double kp=5, ki=0.0, kd=0.01;    


PID Controller(&angle,&Output,&rot_Setpoint,kp,ki,kd,DIRECT);


void setup() {

      Serial.begin(115200);
     
      pinMode(pin_photoint,INPUT);
      linEnc.write(10000); //Choose so that linEnc.read() cant become 0 Zero on intitial reference run, that means bigger than maximum number of possible steps in whole travel
      rotEnc.write(0); //when pendulum ist hanging straight down due to gravity
      delay(500);

     //set Setpoint --> offset by 180 degrees (3600 counts)
     rot_Setpoint=3600;

     
     
     
               //give it a little bump to beginn cause setup_speed is enough power to maintain the movement but not to start it initially
                   go_right(255);
                   delay(10);
           
     //Driving towards the limit switch to reference linear position
               
             while (linEnc.read()!=0){
               
               go_right(setup_speed);
               byte photoint=digitalRead(pin_photoint);
               
               if (photoint==0){
                 break;
                 }
                 
             }
             
             go_stop();
             delay(300);
             linEnc.write(0);


   
   
   
    //Drive to the middel of the travelrange
         //little bump to start with
                   go_left(255);
                   delay(10);

        while (linEnc.read()<3000){    //3000 ist about the middle
               
               go_left(setup_speed);
                       
             }
             
             go_stop();
             delay(500);          

//PID Setup

    Controller.SetMode(AUTOMATIC);
    Controller.SetOutputLimits(-255,255);
    Controller.SetSampleTime(1);
   
   



}


void loop() {
 // put your main code here, to run repeatedly:
angle=rotEnc.read();
lin_position=linEnc.read();
Controller.Compute();
/*if(millis()>5000){
 Controller.SetTunings(kp, 150, kd);
 Serial.println("I active");
}*/

/*Serial.print("angle=");
Serial.print(rot_Setpoint-angle);
Serial.print("\t");
Serial.print("Output=");
Serial.println(Output);
*/


//Serial.println(read_angle());
   if (lin_position>limit_lin_min &&lin_position<limit_lin_max){       //perform as long carriage is within the linear limits
               if (angle<limit_angle_left &&angle>limit_angle_right){    //perform as long as its at least reasonably upright, no point in in trying to balance it when its already hanging down...      



                               
                               //actual controller part
                                   
                                   

                                   
                                   //Serial.println(Output);
                                   
                                   
                                   if(Output<0){
                                     
                                     go_left(map(abs(Output),0,255,setup_speed-10,255));
                                     //go_left(abs(Output));
                                     //Serial.print("L");
                                     //Serial.print("\t");
                                     //Serial.println(map(abs(Output),0,255,setup_speed-10,255));
                                   }


                                   if(Output>0){
                                     
                                     go_right(map(abs(Output),0,255,setup_speed-20,255));
                                     //go_right(abs(Output));
                                     //Serial.print("R");
                                     //Serial.print("\t");
                                     //Serial.println(map(abs(Output),0,255,setup_speed-20,255));
                                   }
                                   
                                   if(Output==0){
                                     go_stop;
                                   }
                               
                                 
                       







           
               }
               else{  
                 go_stop();    //stop carrigage if travel limit is hit
               }
               
     
         
   }
   else{ //drive back into linear limits when they were exeeded
             go_stop();        //stop carriage
             delay(600);
             if(linEnc.read()<limit_lin_min){      //if outside of right limit
                 while(linEnc.read()<limit_lin_min+100){ //drive it back into the travel range, just a little bit over the actual limit
                 go_left(setup_speed+10);
                 }
                 go_stop();
                           
             }
             if(linEnc.read()>limit_lin_max){      //if outside of left limit
                 while(linEnc.read()>limit_lin_max-100){ //drive it back into the travel range, just a little bit over the actual limit
                 go_right(setup_speed+10);
                 }
                 go_stop();
                           
             }
             
   }
   

}
















void go_left(int velocity){       //go left with 'velocity'
  digitalWrite(pin_right,LOW);
  digitalWrite(pin_left,HIGH);
  analogWrite(pin_pwm,velocity);
 
}

void go_right(int velocity){      //go right with 'velocity'
  digitalWrite(pin_right,HIGH);
  digitalWrite(pin_left,LOW);
  analogWrite(pin_pwm,velocity);
 
}

void go_stop(){                   //stop carriage
 analogWrite(pin_pwm,0);
}















jremington

Please edit your post and add code tags ("</>" icon).

DrDiettrich

I'm not sure right now, but I think that a stable (constant) angle is related to an according acceleration (not speed!) of the cart. This acceleration will lead to an increasing speed for any but upright pendulum position.

For stabilization a higher acceleration has to be applied, to swing the pendulum back into upright position. Eventually a move of the cart, at maximum acceleration, for a certain time, can be mapped into the angle being compensated by that move. This will result in kind of a bang-bang regulator, with the on-time depending on the initial declination of the pendulum. The time has to include both initial acceleration and final deceleration into still stand. This regulator also can be used to position the cart e.g. near the center of the path, when first a small movement into opposite direction will make the pendulum swing to that angle, that subsequently is compensated by the timed move (of constant travel length) towards the intended position.

A finer regulation, that does never reach the maximum speed, is desireable but hard to achieve, due to the inertia of the system. I don't think that a 1st degree PID is applicable to this problem (double integral from acceleration into speed, then into distance).


kRz_4tw

Thanks.

Mh, controlling the acceleration instead of the velocity never occured to me. Im afraid this would never lead to a stillstand as well.
By controlling the velocity I have a defined output to stand still. If the acceleration is controled it would basically regulate it self to a stillstand since there is no defined "stand still" output. Even if acceleration=0 is the output the cart would still travel with its left over constant speed.
But maybe I´m misunderstanding that. I cant really follow you on he "bang-bang regulator"-part of your reply. Could you elaborate some more?

I´m still open for other suggestions guys. Keep em coming.

Thanks

DrDiettrich

Your problem deserves knowledge in mechanics and higher level math, in detail about differential equations. Linear control of such a system is not sufficient, as you already found out yourself.

kRz_4tw

Well I certainly have the education concerning the mechanics. Im a mechanical engineer.
I also had to take courses on control systems with the basic mathematics in the Laplace Space.
It wasnt my main focus at the time and it was purely theoretic without much reference on how to actually apply it in real life. But Im picking up on things in those academic papers and recognize things but the general approach to the solution still eludes me.
So try to clue me in, ill try to follow you.

I actually had an idea to basically cascade two PID controllers. Tell me what you think about it.
I´d have a position PID whose output is the setpoint to the angle pid. The angle PID could stay as I have it now.
So if the position PID wants to go left, it will ask the angle PID for an angle to the left. As it gets closer to position, the requested angle will get smaller. If it goes too far, it will ask for an angle in the opposite direction.

Thanks Kevin

DrDiettrich

Okay, you know more about the theory than I do. Perhaps we can learn more from each other :-)

A stable state of the system requires that cart velocity, acceleration and pendulum angle are zero. These are the state values of the system and the controller.

Since the rail length is limited, and you want to control the cart position, the cart position also has to be considered by the controller, i.e. becomes another state variable.

Now the controller requires feedback from the system, where actually the pendulum angle is measured. When the cart is moved by a stepper motor, the cart position is already known, else it has to be measured as well. The remaining parameters, cart speed and acceleration, can be determined from the cart position over time. These parameters should be zero in stable state, but have to be made non-zero temporarily, in order to control the pendulum angle and cart position.

The entire system can be described in time, frequency or state domain. I'm lost with the system identification in time or frequency domain, perhaps you can contribute the equations?

For implementation on a microcontroller I'd go with a bang-bang state regulator, that can be constructed from observation of the system. Such a regulator brings the system into a first (unstable) state, that will bring it "on the road" to the desired position, and a second "bang" will stop it there.

There exist many ways (trajectories) between two positions in time space, depending on the initial declination of the pendulum angle - the higher the declination, the faster the pendulum will move, and the cart has to be moved accordingly. This is where the system limitations (maximum speed...) have to be considered, in order to find a suitable initial declination. With a constant pendulum declination, the cart has to be moved with the according acceleration, what again will hit the system speed limits, probably very soon. I.e. a trajectory of (almost?) constant cart speed would be easier to calculate, so that the angle is reduced continuously - but not necessarily linearily.

Finally the cart has to be stopped, in a way that leaves the system in a stable state, i.e. with zero pendulum angle at zero cart speed, and at the desired final position.

For a practical implementation I have the following ideas:
First the behaviour of the pendulum is determined (measured), when e.g. the cart is moved one (or more) steps aside. The resulting pendulum declination, over time, is recorded.
Then a constant cart speed (step rate) in opposite direction can be applied, that reduces the pendulum declination over time. Recorded is the travel distance or time, over speed, to the point where the pendulum is in upright position again.
Finally the pendulum behaviour is determined, for the case that the cart is stopped from a certain prior speed (second bang). This correction may have to be applied if the pendulum angle is not yet zero, when the cart is already in the desired (final) position.

Then the controller can interpolate between the recorded values, in order to make the cart and pendulum follow the chosen trajectory.

kRz_4tw

Okay, you know more about the theory than I do. Perhaps we can learn more from each other :-)
I wouldnt say that. It´s been a few years and even at the time it´s somehow been like an art class for the math nerd. At least the boring way they presented it to us. I know how to hold the brush but all can draw is a tic tac toe grid...

I´ve gathered some info about bang bang controllers. As far is I understand they wouldnt be a good choice because they are inteded for systems with only two possible outputs, on or off. Besides that the way you descibe it seems to me more like a manual control, in case x that do that, in case y do that and interpolate beween those. This just doesnt seem like the way to go here for me. Just a gut feeling. I also think it wouldnt work well against outside disturbances e.g. poking the pendulum.

What do you think about my suggestion of two PID´s working in series? As far as I can imagine this should work. I actually already implemented it but couldnt come up with a set of sensivities that work yet. Been playing around with it for just a few minutes though so tuning has to be done. I dont even know what the relation between the linear and rotational proportional gains should be. One bigger thatn the oter, by how much. I have to run further tests.

Thanks

Kevin

DrDiettrich

#8
Jul 09, 2016, 04:45 am Last Edit: Jul 09, 2016, 04:48 am by DrDiettrich
Perhaps "dead beat controller" is a better English term for the controller I described. I've learned about such controllers 40 years ago, but never used one since then.

Please try to figure out the equations for the pendulum. The pendulum is subject to gravity (acceleration) and horizontal acceleration from the moving cart. The gravity influence depends on the sine of the pendulum angle, and the angle changes with the inverse function of the effective acceleration. I don't know how this non-linear behaviour can be controlled by one or two PIDs.

Provided that the required acceleration has been calculated, its integral will give the motor speed, as a possible input to the stepper motor library.

kRz_4tw

By your name I suppose your german? Im german so we might speak german if Im correct.

The equations of motions for the inverted pendulum are all over the internet.
But these are several nonlinear second order differential equations. Solving them is beyond my math if its even possible to find an algebraic solution. Probably only a linearisation around the setpoint or a numeric solution is possible.

I think your approaching that to much from a theoretic viewpoint rather then a more practical approach.

I´ve actually come across the wikipedia article for Inverted pendulums.
https://en.wikipedia.org/wiki/Inverted_pendulum

Under Essentials of stabilisation it says
"2. The position of the cart {\displaystyle x} x relative to track center is stabilized by slightly modulating the null angle (the angle error that the control system tries to null) by the position of the cart, that is, null angle {\displaystyle =\theta +kx} =\theta +kx where {\displaystyle k} k is small. This makes the pole want to lean slightly toward track center and stabilize at track center where the tilt angle is exactly vertical. Any offset in the tilt sensor or track slope that would otherwise cause instability translates into a stable position offset. A further added offset gives position control."


This seems to be much like the approach I came up with.

Btw, Im not using a stepper motor. It´s a normal Brushed DC motor. I drive it with a H-Bridge to reverse directions and adjust speed.


Kevin

DrDiettrich

Thanks for the link :-)

If you want to simplify your project, you can try to restrict the capture range to something close to the upright position. But the most effective simplification would be a stepper motor for the cart movement, so that you don't have to fight two different problems at the same time. A DC motor is very non-linear :-(

kRz_4tw

I´m already limiting the capture range to +-400 Steps around the upright position. If it falls any further, the cart is stopped and the pendulum has to be brought into the upright postion by hand again.

I´m working with what I found in the printer. I got the linear quadrature Encoder to tell me the postition of the cart at any time. I actually started this project by basically creating a linear servo motor using a PD controller. I discarded this idea in the process cause I deemed it unnecessary for the inverted pendulum and because I would have to find a way to controll the speed at which the cart went to its desired position as well.

Havent had any time to play with the sensivities in the "cascading PID´s"-approach yet. I might hook me up some potentiometers to adjust the controller gains on the fly and thereby avoid having to recompile and upload every time.

Kevin

kRz_4tw

I´ve been testing around a little. I hooked me up three potentiometers to change gains on the fly so I wont have to recompile every time. That works a treat.
I couldnt come up with a satisfying behavior though. The setting of the linear kp seems difficult. Its either to low so that the pendulums will fall to left if the cart is left of the linear setpoint or to the right when its right of the linear Setpoint.
If I increase lin_kp its able to catch up but the system will slowly start to oscillate around the linear setpoint getting nearer nearer to the end of travel with every cycle. There doesnt seem to be middleground where the oscilation dampens.
I thought the lin_kd might solve this but it´s extremely sensitive. If I cross a certain threshhold it will immediately go nuts wiggling around and eventually running fullspeed torwards the travel limit. The problem isn´t solve if I´m under that threshhold.
I can´t really use a rot_ki or lin_ki because in the time that the pendulum is still hanging down before I initially bring it into the upright by hand, the I terms will build up. This messes up all further controll.

I´m also thinking my general approach might be flawed. Right now the angle controller´s Output is a PWM that gets send (through an L298N H-Bridge) directly to a DC Motor that drives the cart via a belt. First problem there is that the controller
thinks the Output is linear e.g. the speed of the cart at PWM=200 is twice as high as on PWM=100 which is most certainly not the case. Furthermore I actually have to remap the Controller-Output that ranges from -255 to +255 because the motor just
doesnt have enough power unter about PWM 100. Here´s this part in the code.

if(Output<0-output_deadband){
         go_left(map(abs(Output),0,255,setup_speed-25,255));
                                           }
if(Output>0+output_deadband){
        go_right(map(abs(Output),0,255,setup_speed-25,255));
                                            }
if(Output<output_deadband&&Output>-output_deadband){
                                      go_stop;

The setup_speed is 105.

I´m thinking I may have to implement another PID controller that takes this Output from -255 to 255 and establishes a constant velocity for every given Output. That would make the Output linear and the remapping of the Ouput unnecessary.

You might even be right that it´s not actually the velocity of the cart that should be controlled to balance the pendulum but rather its acceleration.

What do you think?


Thanks Kevin

DrDiettrich

If you add an motor controller, you can it make it control either speed or acceleration, based on the same encoder signal.

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy