Using Quaternions to feed a quadcopter PID stabilizing controller to avoid Gimbal lock

Question

I am trying to control my F450 dji quadcopter using a PID controller. From my IMU, I am getting the quaternions, then I convert them to Euler's angles, this is causing me to have the Gimbal lock issue. However, is there a way that I directly use the quaternions to generate my control commands without converting them to Euler's angle?

This conversation here discusses a similar issue but without mentioning a clear answer for my problem.

The three errors so far I am trying to drive to 0 are:

double errorAlpha  = rollMaster  - rollSlave;
double errorTheta  = pitchMaster - pitchSlave;
double errorPsi    = yawMaster   - yawSlave;

where the Master generates the desired rotation and the Slave is the IMU.

UPDATE:

Here are some pieces of my code:

Getting the current and the reference quaternions for bot the Master and the Slave from the ROTATION_VECTOR:

/** Master's current quaternion */
double x   = measurements.get(1);
double y   = measurements.get(2);
double z   = measurements.get(3);
double w   = measurements.get(4);

/** Slave's current quaternion */
double xS  = measurements.get(5);
double yS  = measurements.get(6);
double zS  = measurements.get(7);
double wS  = measurements.get(8);

/** Master's Reference quaternion */
double x0  = measurements.get(9);
double y0  = measurements.get(10);
double z0  = measurements.get(11);
double w0  = measurements.get(12);

/** Slave's Reference quaternion.
 *  If the code has not been initialized yet, save the current quaternion
 *  of the slave as the slave's reference orientation. The orientation of
 *  the slave will henceforth be computed relative to this initial
 *  orientation.
 */
if (!initialized) {
    x0S = xS;
    y0S = yS;
    z0S = zS;
    w0S = wS;
    initialized = true;
}

Then I want to know the orientation of the current quaternion relative to the reference quaternion for both the Master and the Slave.

/**
     * Compute the orientation of the current quaternion relative to the
     * reference quaternion, where the relative quaternion is given by the
     * quaternion product: q0 * conj(q)
     *
     * (w0 + x0*i + y0*j + z0*k) * (w - x*i - y*j - z*k).
     *
     * <pre>
     * See: http://gamedev.stackexchange.com/questions/68162/how-can-obtain-the-relative-orientation-between-two-quaternions
     * http://www.mathworks.com/help/aerotbx/ug/quatmultiply.html
     * </pre>
     */
    // For the Master
    double wr = w * w0 + x * x0 + y * y0 + z * z0;
    double xr = w * x0 - x * w0 + y * z0 - z * y0;
    double yr = w * y0 - x * z0 - y * w0 + z * x0;
    double zr = w * z0 + x * y0 - y * x0 - z * w0;

    // For the Slave
    double wrS = wS * w0S + xS * x0S + yS * y0S + zS * z0S;
    double xrS = wS * x0S - xS * w0S + yS * z0S - zS * y0S;
    double yrS = wS * y0S - xS * z0S - yS * w0S + zS * x0S;
    double zrS = wS * z0S + xS * y0S - yS * x0S - zS * w0S;

Finally, I calculate the Euler angles:

/**
     * Compute the roll and pitch adopting the Tait–Bryan angles. z-y'-x" sequence.
     *
     * <pre>
     * See https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Quaternion_.E2.86.92_Euler_angles_.28z-y.E2.80.99-x.E2.80.B3_intrinsic.29
     * or  http://nghiaho.com/?page_id=846
     * </pre>
     */
    double rollMaster  =  Math.atan2(2 * (wr * xr + yr * zr), 1 - 2 * (xr * xr + yr * yr));
    double pitchMaster =  Math.asin( 2 * (wr * yr - zr * xr));
    double yawMaster   =  Math.atan2(2 * (wr * zr + xr * yr), 1 - 2 * (yr * yr + zr * zr));

and I do the same thing for the Slave.

At the beginning, the reference quaternion should be equal to the current quaternion for each of the Slave and the Master, and thus, the relative roll, pitch and yaw should be all zeros, but they are not!

Answer 1

you should not be seeing a singularity. For the standard roll, pitch, and yaw Euler angles, which are defined by a 3-2-1 rotation from level to the vehicle attitude, there is only a singularity at pitch=+-n*180 +90. This is not a scenario you should be seeing unless you are doing something acrobatic and aggressive. This is an attitude where the vehicle is totally sideways.

Look at how you are converting to Euler angles.

-

I recommend commanding psi like this \begin{equation} \psi_{error} = \left(\psi_{master} - \psi_{master,0} \right) - \left(\psi_{slave} - \psi_{slave,0} \right) \end{equation}