Finding orientation from roll-pitch-yaw-angles

Can someone please give some directions (no pun intended) for calculating the orientation of a two wheel-balancing robot (pitch and yaw, no roll)? I'm not 100% sure where to start, but I've been looking at calculation euler angles with rotation matrices, but I just can't get it to work properly, when I rotate the robot a little and put it back to the starting position the angles are completely off. I haven't implemented a magnetometer yet so I'm just testing this with the gyroscope just to get a understanding of how these calculations work, which angles to multiply with which rotation matrix and so on.

That is all clearly explained here. Start with “Understanding Euler angles”.

jremington: That is all clearly explained here. Start with "Understanding Euler angles".

Thanks, that's actually one of the sources I've been looking for information, but I'm not sure if I understand it correctly.

What I get from the gyroscope is three angles (angular speed integrated over time), I understand these to be roll, pitch and yaw. (In my case the roll == 0). I then understand the angles phi, theta and psi to be relative to the starting position or a "global" angular reference. Should I then multiply these rotational matrices with my rotation vector {roll, pitch, yaw}? That doesn't seem to work.

I thought these rotational matrices are to convert from global angles into roll, pitch and yaw, not the other way around.

If you have a rotation matrix that converts orientations in the fixed coordinate system to the local system, the transpose of that matrix converts orientations in the local system to the fixed system.

In other words the inverse of a rotation matrix is its transpose. R-1=RT

You probably understand this already, but keep in mind that rate gyros alone drift and cannot be used to keep track of orientation in a fixed reference system.

jremington:
If you have a rotation matrix that converts orientations in the fixed coordinate system to the local system, the transpose of that matrix converts orientations in the local system to the fixed system.

In other words the inverse of a rotation matrix is its transpose. R-1=RT

You probably understand this already, but keep in mind that rate gyros alone drift and cannot be used to keep track of orientation in a fixed reference system.

I still don’t quite get it.

In this link “Understanding Euler Angles |”, in the section “The rotation matrix for moving the opposite direction - from the body frame to the inertial frame is given by:”
R(phi, theta, psi) = R(-psi) * R(-theta) * R(-phi) and they show the full matrix with all the elements. Now, should I substitute phi = roll, theta = pitch and psi = yaw here and then multiply this (3x3) matrix with an array (3x1) consisting of Roll pitch and yaw?

There are only two reference frames of interest, the body frame and the inertial frame.

In that document, the inertial frame is NED (North = x axis, East = y axis and Down = z axis). You might define a point (x,y,z) in that frame, relative to some origin.

Then to convert the (x,y,z) coordinate in the inertial frame to (x’, y’, z’) in the body frame (defined by roll, pitch and yaw angles with respect to NED), multiply (x,y,z) by the appropriate rotation matrix (RBI) to get (x’, y’, z’).

To convert (x’, y’, z’) in the body frame back to the inertial frame, multiply by the transpose of that rotation matrix (RIB).

If you look carefully, RBI and RIB are indeed transposes of each other.

jremington:
There are only two reference frames of interest, the body frame and the inertial frame.

In that document, the inertial frame is NED (North = x axis, East = y axis and Down = z axis). You might define a point (x,y,z) in that frame, relative to some origin.

Then to convert the (x,y,z) coordinate in the inertial frame to (x’, y’, z’) in the body frame (defined by roll, pitch and yaw angles with respect to NED), multiply (x,y,z) by the appropriate rotation matrix (RBI) to get (x’, y’, z’).

To convert (x’, y’, z’) in the body frame back to the inertial frame, multiply by the transpose of that rotation matrix (RIB).

If you look carefully, RBI and RIB are indeed transposes of each other.

Okay, a little more clear now. But should I do the sine and cosine of the gyro-angles in the rotation matrices before multiplying it with the gyro-angles again, or should I do sine and cosine of the “global” angles?

I will post code of what I’ve done (and which doesn’t work) when I get home from work.

jremington:
There are only two reference frames of interest, the body frame and the inertial frame.

In that document, the inertial frame is NED (North = x axis, East = y axis and Down = z axis). You might define a point (x,y,z) in that frame, relative to some origin.

Then to convert the (x,y,z) coordinate in the inertial frame to (x’, y’, z’) in the body frame (defined by roll, pitch and yaw angles with respect to NED), multiply (x,y,z) by the appropriate rotation matrix (RBI) to get (x’, y’, z’).

To convert (x’, y’, z’) in the body frame back to the inertial frame, multiply by the transpose of that rotation matrix (RIB).

If you look carefully, RBI and RIB are indeed transposes of each other.

Some of the code is posted below. I’ve checked the angles from the gyro (angle[0,1,2]) and when moving the robot around a single axis it does report the angles precisely and returns back to 0 when put back. This also happens with angle_x, angle_y and angle_z. If I move it about 2 or 3 axes it doesn’t return back to 0 when put back.

unsigned int interval = 10;
unsigned long currentTime, previousTime = 0;

void loop() {
  if (Serial.available() > 1) { // For resetting gyro
    char inData = Serial.read();
    if (inData == 13) {
      angle[0] = 0;
      angle[1] = 0;
      angle[2] = 0;
    }
  }
  currentTime = millis();
  if (currentTime - previousTime > interval) {
    previousTime += interval;
    myIMU.getGyroDPS(gyro); // get angles from gyro in deg/s

    for (byte i = 0; i < 3; i++) {
      angle[i] += deg2rad(gyro[i]) * (float)interval / 1000.0 ;;
    }

    //  Sensor is mounted with y-axis pointing forward, x-axis to the right and z-axis up.
    //  Transforms this to x-axis pointing forward, y-axis to the right and z-axis down:
    float phi   =  angle[1];
    float theta =  angle[0];
    float psi   = -angle[2];

    float angle_x = cos(psi) * cos(theta) * phi + (cos(psi) * sin(phi) * sin(theta) - cos(phi) * sin(psi)) * theta    + (sin(phi) * sin(psi) + cos(phi) * cos(psi) * sin(theta)) * psi;
    float angle_y = cos(theta) * sin(psi) * phi + (cos(phi) * cos(psi) + sin(phi) * sin(psi) * sin(theta)) * theta + (cos(phi) * sin(psi) * sin(theta) - cos(psi) * sin(phi)) * psi;
    float angle_z =         -sin(theta) * phi +               cos(theta) * sin(phi) * theta                   +   cos(phi) * cos(theta) * psi;

    Serial.print(rad2deg(angle_x)); Serial.print(", ");
    Serial.print(rad2deg(angle_y)); Serial.print(", ");
    Serial.println(rad2deg(angle_z));

    //    Serial.print(rad2deg(angle[0])); Serial.print(", ");
    //    Serial.print(rad2deg(angle[1])); Serial.print(", ");
    //    Serial.println(rad2deg(angle[2]));
  }
}

Gyros drift and cannot be used to establish absolute orientation.

Keep in mind that the order in which you do rotations about the different axes matters a great deal. The rotation matrix you have entered is defined for only one particular order of rotations and won't work for any other.

jremington: Gyros drift and cannot be used to establish absolute orientation.

Keep in mind that the order in which you do rotations about the different axes matters a great deal. The rotation matrix you have entered is defined for only one particular order of rotations and won't work for any other.

I have calibrated the gyros, but the calculated angles still drift approx 0.3deg per 10 seconds or so. It's pretty stable, at least stable enough to test it for a couple of seconds.

I don't really understand how the order of rotations matter, because the rotations about two or three axes almost always happen at the same time, for instance a quadcopter (or other multiroter craft) almost never rotates only about one single axis, or in a predefined order of angles?

I don't really understand how the order of rotations matter

Try it with a book.

Rotate the book first 90 degrees clockwise about X (North), then 90 degrees clockwise about Y (East).

Is the book's final orientation the same, if you rotate first 90 degrees clockwise about Y, then X?

jremington: Try it with a book.

Rotate the book first 90 degrees clockwise about X (North), then 90 degrees clockwise about Y (East).

Is the book's final orientation the same, if you rotate first 90 degrees clockwise about Y, then X?

Yes, that I understand, but in most cases objects are not rotated around only one axis at a time, for instance in multirotors or aircrafts in general?

The mathematical framework you are using to represent a rotational transformation requires breaking that transformation down into successive rotations about three axes. There are many ways of doing that breakdown.

There are other frameworks for representing rotational transformations, for example a quaternion represents a rotational transformation by a single angle about an axis that points in a carefully chosen direction.

jremington: The mathematical framework you are using to represent a rotational transformation requires breaking that transformation down into successive rotations about three axes. There are many ways of doing that breakdown.

There are other frameworks for representing rotational transformations, for example a quaternion represents a rotational transformation by a single angle about an axis that points in a carefully chosen direction.

Do you happen to know how these rotations are broken down so I can represent the rotations relative to the intertial frame?