3
$\begingroup$

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!

Improve this question
$\endgroup$
4
  • 1
    $\begingroup$ What data do you have that shows you are having gimbal lock? What are your symptoms? How are you converting to Euler angles? I ask because, to get to gimbal lock, two of your motion axes need to coincide. This means that generally either the roll or pitch axis has to get to 90 degrees to coincide with yaw, but a quadcopter at 90 degrees of roll or pitch doesn't have any lift. $\endgroup$
    – Chuck ♦
    Jun 28, 2016 at 18:27
  • $\begingroup$ For the Master and the Slave units, I first calculate the relative quaternion for each one (to know the new orientation of each), and then I use the formulas in this link to calculate the roll, pitch, and yaw of the Master and the Slave, then I calculate the errors as shown above. When I first run the system, i.e. without moving the Master nor the Slave, I get zeros for the rolls and pitches (as expected) but not for the yaws. That's my problem. $\endgroup$
    – M.A.
    Jun 30, 2016 at 7:51
  • $\begingroup$ Can you post a block diagram or code showing what you're doing? How are you calculating the quaternions to start with? What are you expecting the yaws to be, what are you getting, and how are you initializing them? This sounds like a problem either in calculating the quaternion and back or in initializing the code. If roll and pitch are both zero then you are probably not in gimbal lock. (Depending on the order of rotations you take) $\endgroup$
    – Chuck ♦
    Jun 30, 2016 at 13:00
  • $\begingroup$ I already updated my question above. Please have a look. $\endgroup$
    – M.A.
    Jun 30, 2016 at 14:35

1 Answer 1

Reset to default
2
$\begingroup$

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}

Improve this answer
$\endgroup$
10
  • $\begingroup$ I've checked the quaternion to Euler angles conversion formulas many times actually. Do you see something wrong there? $\endgroup$
    – M.A.
    Jul 1, 2016 at 8:01
  • $\begingroup$ its possible they are correct, just near a singularity. if this is the case you should construct a rotation matrix that will transform your desired level state to be 0 pitch and zero roll. Pre-multiply this rotation matrix by the rotation matrix constructed from the quaternions. The final matrix will allow you to recover the expected Euler angles. $\endgroup$
    – holmeski
    Jul 1, 2016 at 17:27
  • $\begingroup$ The rotation matrix I get from the quaternions already transforms my desired level state to be 0 pitch and 0 roll. My current problem is with the relative yaw, which is non-zero. I can't see how your suggestion would solve this issue. $\endgroup$
    – M.A.
    Jul 3, 2016 at 14:56
  • $\begingroup$ are you initializing you master yaw with the yaw from the imu? The imu yaw will likely be driven by a magnetometer ( if it has one). If there is no magnetometer there will be drift in the imu yaw estimates because it will naively be integrating measured yaw-rate without any method for correcting bias in the gyro $\endgroup$
    – holmeski
    Jul 3, 2016 at 17:46
  • 1
    $\begingroup$ i think its time for a new question, or at least some revisions in your initial question $\endgroup$
    – holmeski
    Jul 4, 2016 at 16:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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