2
$\begingroup$

Good day,

I am working on an autonomous flight controller for a quadcopter ('X' configuration) using only angles as inputs for the setpoints used in a single loop PID controller running at 200Hz (PID Implementation is Here: Quadcopter PID Controller: Derivative on Measurement / Removing the Derivative Kick). For now I am trying to get the quadcopter to stabilize at a setpoint of 0 degrees. The best I was able to come up with currently is +-5 degrees which is bad for position hold. I first tried using only a PD controller but since the quadcopter is inherently front heavy due to the stereo cameras, no amount of D or P gain is enough to stabilize the system. An example is the image below which I added a very small I gain:

Small/Almost Negligible I-gain

As you can see from the image above (at the second plot), the oscillations occur at a level below zero degrees due to the quadcopter being front heavy. This means that the quad oscillates from the level postion of 0 degrees to and from a negative angle/towards the front. To compensate for this behaviour, I discovered that I can set the DC level at which this oscillations occur using the I gain to reach the setpoint. An image is shown below with [I think] an adequate I gain applied:

Ok I-gain, I think

I have adjusted the PID gains to reduce the jitters caused by too much P gain and D gain. These are my current settings (Which are two tests with the corresponding footage below):

Current Settings

Test 1: https://youtu.be/8JsraZe6xgM

Test 2: https://youtu.be/ZZTE6VqeRq0

I can't seem to tune the quadcopter to reach the setpoint with at least +-1 degrees of error. I noticed that further increasing the I-gain no longer increases the DC offset.

When do I know if the I-gain I've set is too high? How does it reflect on the plot?

EDIT: The Perr in the graphs are just the difference of the setpoint and the CF (Complementary Filter) angle. The Derr plotted is not yet divided by the deltaTime because the execution time is small ~ 0.0047s which will make the other errors P and I hard to see. The Ierr plotted is the error integrated with time.

All the errors plotted (Perr, Ierr, Derr) are not yet multiplied by the Kp, Ki, and Kd constants

The 3rd plot for each of the images is the response of the quadcopter. The values on the Y axis correspond to the value placed as the input into the gpioPWM() function of the pigpio library. I had mapped using a scope the values such that 113 to 209 pigpio integer input corresponds to 1020 to 2000ms time high of the PWM at 400Hz to the ESC's

EDIT:

Here is my current code implementation with the setpoint of 0 degrees:

cout << "Starting Quadcopter" << endl;

float baseThrottle = 155; //1510ms
float maxThrottle = 180; //This is the current set max throttle for the PITCH YAW and ROLL PID to give allowance to the altitude PWM. 205 is the maximum which is equivalent to 2000ms time high PWM 
float baseCompensation = 0; //For the Altitude PID to be implemented later


delay(3000);

float startTime=(float)getTickCount();
deltaTimeInit=(float)getTickCount(); //Starting value for first pass

while(1){
//Read Sensor Data
readGyro(&gyroAngleArray);
readAccelMag(&accelmagAngleArray);

//Time Stamp
//The while loop is used to get a consistent dt for the proper integration to obtain the correct gyroscope angles. I found that with a variable dt, it is impossible to obtain correct angles from the gyroscope.

while( ( ((float)getTickCount()-deltaTimeInit) / ( ((float)getTickFrequency()) ) ) < 0.005){ //0.00209715|0.00419
       deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
       cout << " DT endx = " << deltaTime2 << endl;
}
//deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
deltaTimeInit=(float)getTickCount(); //Start counting time elapsed
cout << " DT end = " << deltaTime2 << endl;


//Complementary Filter
float pitchAngleCF=(alpha)*(pitchAngleCF+gyroAngleArray.Pitch*deltaTime2)+(1-alpha)*(accelmagAngleArray.Pitch);
float rollAngleCF=(alpha)*(rollAngleCF+gyroAngleArray.Roll*deltaTime2)+(1-alpha)*(accelmagAngleArray.Roll);
float yawAngleCF=(alpha)*(yawAngleCF+gyroAngleArray.Yaw*deltaTime2)+(1-alpha)*(accelmagAngleArray.Yaw);


//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchError - pitchPrevError;
pitchPrevError = pitchError;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollError - rollPrevError;
rollPrevError = rollError;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawError - yawPrevError;
yawPrevError = yawError;


//PID controller list
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum + pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum + rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum + yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing    
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

//Motor Front Right (2)
float motorPwm2 =  -pitchPID - rollPID + yawPID + baseThrottle + baseCompensation; 

//Motor Back Left (3)
float motorPwm3 = pitchPID + rollPID + yawPID + baseThrottle + baseCompensation; 

//Motor Back Right (4)
float motorPwm4 = pitchPID - rollPID - yawPID + baseThrottle + baseCompensation;


 //Check if PWM is Saturating - This method is used to fill then trim the outputs of the pwm that gets fed into the gpioPWM() function to avoid exceeding the earlier set maximum throttle while maintaining the ratios of the 4 motor throttles. 
    float motorPWM[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float minPWM = motorPWM[0];
    int i;
    for(i=0; i<4; i++){ // Get minimum PWM for filling
        if(motorPWM[i]<minPWM){
            minPWM=motorPWM[i];
        }
    }

    cout << " MinPWM = " << minPWM << endl;

    if(minPWM<baseThrottle){
        float fillPwm=baseThrottle-minPWM; //Get deficiency and use this to fill all 4 motors
        cout << " Fill = " << fillPwm << endl;
        motorPwm1=motorPwm1+fillPwm;
        motorPwm2=motorPwm2+fillPwm;
        motorPwm3=motorPwm3+fillPwm;
        motorPwm4=motorPwm4+fillPwm;
    }

    float motorPWM2[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float maxPWM = motorPWM2[0];
    for(i=0; i<4; i++){ // Get max PWM for trimming
        if(motorPWM2[i]>maxPWM){
            maxPWM=motorPWM2[i];
        }
    }

    cout << " MaxPWM = " << maxPWM << endl;

    if(maxPWM>maxThrottle){
        float trimPwm=maxPWM-maxThrottle; //Get excess and use this to trim all 4 motors
        cout << " Trim = " << trimPwm << endl;
        motorPwm1=motorPwm1-trimPwm;
        motorPwm2=motorPwm2-trimPwm;
        motorPwm3=motorPwm3-trimPwm;
        motorPwm4=motorPwm4-trimPwm;
    }

//PWM Output
    gpioPWM(24,motorPwm1);  //1
    gpioPWM(17,motorPwm2);  //2
    gpioPWM(22,motorPwm3);  //3
    gpioPWM(18,motorPwm4);  //4
$\endgroup$
  • $\begingroup$ Have you tried this with a longer leash? It looks like your vehicle is sitting (some weight) on the tether and not fully in the air. Also it looks like you're actively pulling on the tether. Both of these means the tether is forcing your vehicle and maybe that's why it's not stabilizing. Also, I don't see the reference command in your plots - is it zero? And what is 'cf'? $\endgroup$ – Chuck Feb 2 '16 at 22:33
  • $\begingroup$ @Chuck the test is just with the PItch PID active. The green ropes are just the limit the quadcopter's movement to +-45 degrees on the earlier tests. They actually serve no purpose now since the quad oscillates at +- 5 degrees. There is no weight placed to tether the quadcopter. I am pulling on the string to increase the tension since some of the bounce that occurs due to low tension causes instability. Doing this also helps in actually isolating the movement of the quadcopter in one axis only. The setpoint for tuning is zero degrees. $\endgroup$ – user123456098 Feb 2 '16 at 22:40
  • $\begingroup$ cf stands for complementary filter $\endgroup$ – user123456098 Feb 2 '16 at 22:54
  • $\begingroup$ @Chuck I just read over your reply again. I misread this statement: "It looks like your vehicle is sitting (some weight) on the tether and not fully in the air." I adjusted the throttle such that its enough for it to balance it self while taking care to leave room for the Throttle/ALtitude PID. Should have I adjusted the throttle first such that its already lifting itself before adjusting the PID values? Here's a video to illustrate my current set up ( youtu.be/LHEzem0QJzQ ). I took some of Ian's previous advice and tightened the ropes such that I need not to pull on them. $\endgroup$ – user123456098 Feb 3 '16 at 23:17
  • $\begingroup$ Will also try to find a way to mount a pole in the center of the quad's battery compartment. $\endgroup$ – user123456098 Feb 3 '16 at 23:27
2
$\begingroup$

Based on the videos, it looks like the answer is, unfortunately, "the test setup you have might be insufficient to say one way or the other".

There is too much slack in the tethers to draw any meaningful conclusions about whether your code is doing the right thing. The quadcopter seemed to be reacting somewhat correctly to the way it was being tugged by the ropes. There were some moments where it looked incorrect as well, but they were not isolated enough to point to a specific logic error.

Spend some time on the quality and stability of your test setup so that you can rigidly constrain two axes while letting the other move freely. (For example, disable one pair of motors and hang the quadcopter from strings around those propeller shafts.) That will let you troubleshoot one axis at a time in a very reasonable way, and provide more accurate feedback on the correctness of your PID values.

Looking at your setup, another problem seems to be that you're attaching the tether too low and creating an "inverted pendulum" effect. (See also, this question on quadcopter center of lift).

enter image description here

You need to create a mounting bracket for your tether that will rigidly mount it to the quadcopter body at the level of the center of lift that the propellers provide. It would also help to use something structural (like a metal post vertically attached to a sturdy base), instead of just rope or wire.

$\endgroup$
  • $\begingroup$ should I increase the tension on the strings? I am actually confused on how to do this procedure because on some people who did tunings this way using strings, they first made sure that the base throttle was lifting the quadcopter before adjusting PID values. However, others who also used strings did not do the latter (adjusting the throttle first) but instead just let the quadcopter balance on it's own weight. Also I cant disable one pair of motors since they are both required for one axis since I am using an 'X' quadcopter configuration $\endgroup$ – user123456098 Feb 3 '16 at 6:53
  • $\begingroup$ Examples of each method (strings) are: 1. Letting the quadcopter balance on its own. youtube.com/watch?v=3650JlYrp7E 2. Initiating throttle first. youtube.com/watch?v=fYUQgmkYi5o $\endgroup$ – user123456098 Feb 3 '16 at 6:58
  • $\begingroup$ I've updated my answer to address this. $\endgroup$ – Ian Feb 3 '16 at 16:49
  • $\begingroup$ Will try this thank you :) I will try to narrow down other possible problems as well. I am currently trying higher I gains. $\endgroup$ – user123456098 Feb 3 '16 at 23:03
  • $\begingroup$ How did things turn out? Do you have a picture of the working test setup? $\endgroup$ – Ian Mar 4 '16 at 17:46
1
$\begingroup$

What does your code look like that you use to calculate your errors? I had suggestions to revise your code, but you're just linking back to your earlier questions so I don't know if that's current or not.

If you are calculating your error terms correctly, you could try an experimental tuning method like Ziegler-Nichols, but again, this relies on your error terms to be calculated correctly.

In general, the integral term is used to "accumulate" error such that there is ultimately no steady state error. If the integral gain is too low then it will take a long time to reach (near) zero steady state error, but if it's too high then you will experience overshoot and oscillations as your controller overreacts to minor error accumulation.

$\endgroup$
  • $\begingroup$ I posted my code implementation. I followed your suggestion to move the time stamp however I added a while loop to place a consistent delay to obtain a consistent integration when obtaining the gyroscope angles such that it converges(accelerometer is slower) with the accelerometer angles at no throttle. $\endgroup$ – user123456098 Feb 2 '16 at 22:20
  • $\begingroup$ I found that when using a variable dt, the gyroscope drifts like crazy making it impossible to get accurate orientation measurements $\endgroup$ – user123456098 Feb 2 '16 at 22:29

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.