How to implement PD controller to this 2D planar quadcopter dynamics?

Question

I want to code the dynamics of 2D planar quadrotor and than control it to drive it from one state to another.

Dynamics that I use is taken from the online course fiven by Vijay Kumar in Coursera as follows,

$ \begin{bmatrix} \ddot{y}\\ \ddot{z}\\ \ddot{\phi} \end{bmatrix} = \begin{bmatrix} 0\\ -g\\ 0 \end{bmatrix} + \begin{bmatrix} -\frac{1}{m}sin\phi & 0\\ \frac{1}{m}cos\phi & 0\\ 0 & -\frac{1}{I_{xx}} \end{bmatrix}\begin{bmatrix} u_1\\ u_2 \end{bmatrix} $

it has some linearizations also as $sin\phi->\phi$ & $cos\phi -> const.$

And u1, u2 is defined by;

$u_1=m\{g+\ddot{z}_T(t)+k_{v,z}*(\dot{z}_T(t)-\dot{z})+k_{p,z}*(z_{T}(t)-z)\}$

$u_2=I_{xx}(\ddot{\phi}+k_{v,\phi}*(\dot{\phi}_T(t)-\dot{\phi})+k_{p,\phi}*(\phi_{T}(t)-\phi))$

$\phi_c=-\frac{1}{g}(\ddot{y}_T(t)+k_{v,y}*(\dot{y}_T(t)-\dot{y})+k_{p,y}*(y_{T}(t)-y))$

it is assumed to be the vehicle is near hover condition and commanded roll angle $\phi_c$ is calculated based on desired y-component and is used to calculate u2 which is net moment acting on CoG.

The thing that I dont understand is, don't I need any saturation on actuators? Do I need to implement some limiting part on my code to limit the control signals.

The other thing is, I don't have any desired acceleration. There is those terms in control signal equations. Can I remove them?

The last thing is, my control signals creates some signals causes to vehicle to have order of 10^5 in roll angle by integrating the high angular rates caused by high u2 moment signal I guess. Since the linearization works on small angle approximation, those high angles and rates are problematic. Thus how can I handle it?

Answer 1

Yes, you will have saturations on your torque and thrust outputs that you will send to your motors after the mapping. However this will be machine specific and needs to be handled in your code. You may also require saturations to maximum pitch angle as this PD controller will not handle large angles very well.

The desired acceleration is a feed forward term carried over from the derivatives of the trajectory that you want to pursue. Yes, you can safely ignore this and still converge to the desired setpoint/trajectory albeit with lower performance.

You should re-tune your gains and add the correct saturations so that your simulation makes sense physically. And make sure that you use a complementary filter to estimate your orientation as simply integrating the gyroscope rates will cause drift in your states. Despite this you will find it hard to control a quadrotor with just PD as you may have model uncertainties and disturbances. Some researchers have developed controllers which map your angle errors in SO(3) preventing singularities and allowing you to reach the harder orientations.