0
$\begingroup$

I am working on this short assignment where I have to find the right PD values for a simulated quadcopter. I have to reach 0.9 meters under 1 second with an overshoot of less than 5%. This is my code:

function [ u ] = pd_controller(~, s, s_des, params)
%PD_CONTROLLER  PD controller for the height
%
%   s: 2x1 vector containing the current state [z; v_z]
%   s_des: 2x1 vector containing desired state [z; v_z]
%   params: robot parameters

% u = 0;

error = s_des - s;
z_des = 0.9;

Kp = 0.5;
Kv = 1.0;

u = params.mass*(z_des + Kp*error(1) + Kv*error(2) + params.gravity);

end

So far this is my output:

enter image description here

I've tried many different value combinations and so far this is the closest that I could get.

Improve this question
$\endgroup$
4
  • 1
    $\begingroup$ Why are you adding z_des to the control? That looks wrong. $\endgroup$ – holmeski Aug 2 '20 at 21:50
  • $\begingroup$ @holmeski Initially it was in the starter code. Now that I removed it, the drone does not lift off at all. $\endgroup$ – oo92 Aug 2 '20 at 21:52
  • $\begingroup$ If you set the control to m*(g + I), what's the lowest I can be while still driving the vehicle upwards? $\endgroup$ – holmeski Aug 2 '20 at 22:04
  • $\begingroup$ z_des seems to be a feedforward value. $\endgroup$ – jdios Mar 26 at 13:52
2
$\begingroup$

This problem can be conveniently solved by resorting to a state-feedback controller as the one depicted below.

state-feedback

In this case, we have that $P=\frac{1}{m \cdot s^2}$, $x=\left[ z, \dot{z} \right]^T$, $K=\left[k_p, k_d\right]$, $k_f=\frac{k_p}{m}$. Then, we need to apply the feed-forward term $v=mg$ to counteract the gravity.

We end up with the transfer function of the closed-loop system:

$$ \frac{y(s)}{r(s)}=\frac{\frac{k_p}{m}}{s^2+\frac{k_d}{m}s+\frac{k_p}{m}}. $$

This is a well-known and well-studied second-order system given in the form

$$ \frac{\omega_n^2}{s^2+2\xi\omega_ns+\omega_n^2}, $$

where the parameters $\omega_n$ and $\xi$ are expressed as functions of the gains $k_p$, $k_d$ and the mass $m$ as $\omega_n=\sqrt\frac{k_p}{m}$ and $\xi=\frac{k_d}{2\sqrt{m \cdot k_p}}$.

To meet the given requirements, have a look at the information you can peruse for example at https://www.javatpoint.com/control-system-time-response-of-second-order-system, where the overshoot and the settling-time are provided by the following expressions: $$ \begin{cases} e^\frac{\pi\xi}{\sqrt{1-\xi^2}}=0.05 \\ \frac{4}{\xi\omega_n}=1 \end{cases}. $$

The gains $k_p$ and $k_d$ can be thus readily determined from the formulas above through substitution.

For a unitary mass $m=1$ kg, it comes out that $k_d=8$ Ns/m and $k_p=k_f=33.596$ N/m.

Here's the step response:

step-response

The step response is generally given with respect to a unitary input step; however, the system is linear and thus this behavior is preserved also for an input step of amplitude $0.9$ m.

To conclude, your controller's output shall be:

u = params.mass*params.gravity - kp*z - kd*v_z + kf*z_des;
Improve this answer
$\endgroup$
0
$\begingroup$

You are controlling a second order system. Set both kp and kv to 4.

If the system does not reach your desired altitude then there are additional problems with the code.

Improve this answer
$\endgroup$
5
  • $\begingroup$ It doesn't work. $\endgroup$ – oo92 Aug 3 '20 at 0:15
  • $\begingroup$ It is likely that there is an error in your setup. Those gains should see the natural frequency to 2 with a damping ratio of 1. Are you sure that you're setting the mass correctly? $\endgroup$ – holmeski Aug 3 '20 at 0:55
  • $\begingroup$ The code you see above is all I got $\endgroup$ – oo92 Aug 3 '20 at 1:05
  • 1
    $\begingroup$ I recommend checking out my comment to your question. Find the smallest u that will drive the quad off the ground. Start with u=mg then u=mg +.1 and so on until you find some value , let's call it k, that will just drive the quad off the ground. Then try the control law u=m*(kp e(1) + kve(2) +g) + k. That should get you close to the desired error. $\endgroup$ – holmeski Aug 3 '20 at 2:10
  • 1
    $\begingroup$ Also, have you checked all the values to make sure they make sense? Do the desired position and velocity make sense? Is the mass real? $\endgroup$ – holmeski Aug 3 '20 at 2:13

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.