Decomposing rotation matrix (x,y',z'') - Cartesian angles

问题

Decomposing rotation matrix (x,y',z'') - Cartesian angles

Im currently working with rotation matrices and I have the following problem: Given three coordinate systems (O0,x0,y0,z0; O1,x1,y1,z1; O2,x2,y2,z2) which coincide. We rotate first the frame #1 with the respect to frame #0, then the frame #2 with respect to frame #1.

The order of the rotations: R = Rx_alpha * Ry_beta * Rz_gamma, so first about x, then y', then z'', which are also known as the Cartesian angles. If R1 stands for the 1st and R2 for the 2nd rotation, we are looking for the angles of the 2nd frame with respect to initial frame (#0) after both of the rotations. This can be done by decomposing the rotation matrix R (where:R = R1*R2 ). There are many literature available, how it can be done by Euler- and RPY-angles, but I don't find any, how to solve this problem in case of Cartesian angles.

I have a matlab function which works only by simple rotations. If all the angles have values different than 0 (example below), then the result becomes really unstable.

Orientation of the 1st frame with respect to the frame #0:

    alpha1 = 30*pi/180;
    beta1 = 10*pi/180;
    gamma1 = 0*pi/180;

Orientation of the 2nd frame with respect to the frame #1

    alpha2 = 10*pi/180;
    beta2 = 10*pi/180;
    gamma2 = 0*pi/180;

The matlab function I was using for solving the problem:

function [q] = cartesian_angles(R)

beta = asin(R(1,3));

*% Catching the numerical singularty*
if abs(abs(beta)-pi/2) > eps;
    *% singulartiy of acos*
    gamma1 = acos(R(1,1) / cos(beta));
    gamma2 = asin(-R(1,2) / cos(beta));
    if gamma2<0
        gamma=2*pi-gamma1;
    else
        gamma=gamma1;
    end
    alpha1 = acos(R(3,3) / cos(beta));
    alpha2 = asin(-R(2,3) / cos(beta));
    if alpha2<0
        alpha = 2*pi-alpha1;
    else
        alpha = alpha1;
    end
else
    fprintf('beta=pi/2 \n')
    gamma = 0;
    alpha = 0;
    beta  = 0;
end;

alpha = alpha*180/pi;
beta = beta*180/pi;
gamma = gamma*180/pi;

q = [alpha; beta; gamma];

Thank you for any help! If you have some questions don't hesitate to ask!

Marci

回答1:

First, I'm going to assume you are passing into your function a well conditioned, right-handed rotation matrix. I'm going to use the same rotation sequence as you listed above, X Y' Z''

If you know the symbolic construction of the rotation matrix you are trying to extract angles from, the math is pretty straight forward. Below is an example of matlab code to determine the construction of the rotation matrix of order X-Y'-Z''

a = sym('a');%x
b = sym('b');%y
g = sym('g');%z

Rx = [1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz = [cos(g) -sin(g) 0;sin(g) cos(g) 0;0 0 1];

R = Rz*Ry*Rx

The output looks like this:

R =

[ cos(b)*cos(g), cos(g)*sin(a)*sin(b) - cos(a)*sin(g), sin(a)*sin(g) + cos(a)*cos(g)*sin(b)]
[ cos(b)*sin(g), cos(a)*cos(g) + sin(a)*sin(b)*sin(g), cos(a)*sin(b)*sin(g) - cos(g)*sin(a)]
[       -sin(b),                        cos(b)*sin(a),                        cos(a)*cos(b)]

Here's the same result in a nicer looking format:

Now let's go over the math to extract the angles from this matrix. Now would be a good time to become comfortable with the atan2() function.

First solve for the beta angle (by the way, alpha is the rotation about the X axis, beta is the rotation about Y' axis, and gamma is the angle about the Z'' axis):

beta = atan2(-1*R(3,1),sqrt(R(1,1)^2+R(2,1)^2))

Written more formally,

Now that we have solved for the beta angle we can solve more simply for the other two angles:

alpha = atan2(R(3,2)/cos(beta),R(3,3)/cos(beta))
gamma = atan2(R(2,1)/cos(beta),R(1,1)/cos(beta))

Simplified and in a nicer format,

The above method is a pretty robust way of getting the Euler angles out of your rotation matrix. The atan2 function really makes it much simpler.

Finally I will answer how to solve for the rotation angles after a series of rotations. First consider the following notation. A vector or rotation matrix will be notated in the following way:

Here "U" represents the universal frame, or global coordinate system. "Fn" represents the nth local coordinate system that is different from U. R means rotation matrix (this notation could also be used for homogeneous transformations). The left side superscript will always represent the parent frame of reference of the rotation matrix or vector. The left side subscript indicates the child frame of reference. For example, if I have a vector in F1 and I want to know what it is equivalently in the universal frame of reference I would perform the following operation:

To get the vector resolved in the universal frame I simply multiplied it by the rotation matrix that transforms things from F1 to U. Notice how the subscripts are "cancelled" out by the superscript of the next item in the equation. This is a clever notation to help someone from getting things mixed up. If you recall, a special property of well conditioned rotation matrices is that the inverse matrix is the transpose of the matrix, which is will also be the inverse transformation like this:

Now that the notation details are out of the way, we can start to consider solving for complicated series of rotations. Lets say I have "n" number of coordinate frames (another way of saying "n" distinct rotations). To figure out a vector in the "nth" frame in the universal frame I would do the following:

To determine the Cardan/Euler angles that result from "n" rotations, you already know how to decompose the matrix to get the correct angles (also known as inverse kinematics in some fields), you simply need the correct matrix. In this example I am interested in the rotation matrix that takes things in the "nth" coordinate frame and resolves them into the Universal frame U:

There is it, I combined all the rotations into the one of interest simply by multiplying in the correct order. This example was easy. More complicated cases come when someone wants to find the reference frame of one rigid body resolved in the frame of another and the only thing the two rigid bodies have in common is their measurement in a universal frame.

I want to also note that this notation and method can also be used with homogeneous transformations but with some key differences. The inverse of a rotation matrix is its transpose, this is not true for homogeneous transformations.

回答2:

Thank you for you answer willpower2727, your answer was really helpful!

But I would like to mention, that the code you have shown is useful to decompose rotational matrices, which are built in the following way:

R = Rz*Ry*Rx

What I'm looking for:

R = Rx*Ry*Rz

Which results into the following rotational matrix:

Rotational Matrix

However, it's not a problem, as following the method how you calculate the angles alpha, beta and gamma, it was easy to modify the code so it decomposes the matrix shown above.

The angles:

beta = atan2(  R(1,3), sqrt(R(1,1)^2+(-R(1,2))^2) )
alpha = atan2( -(R(2,3)/cos(beta)),R(3,3)/cos(beta) )
gamma = atan2( -(R(1,2)/cos(beta)),R(1,1)/cos(beta) )

One thing is still not clear though. The method is perfectly useful, bunt only if I calculate the angles after one rotation. As there are more rotations linked after each other, the results are false. However, it's still solvable, I guess, considering the following way: Let's say, we have two rotations linked after each other (R1 and R2). q1 shows the angles of R1, q2 of R2. after decomposing the single matrices. The total angle of rotation of the matrix R=R1*R2can be easily calculated through summing up the rangles before: q=q1+q2

Is there no way, how to calculate the angles of the total rotation, not by summing the partial angles, but decomposing the matrix R=R1*R2?

UPDATE:

Considering the following basic example. The are to rotations linked after each other:

a1 = 10*pi/180
b1 = 20*pi/180
g1 = 40*pi/180
R1 = Rx_a1*Ry_b1_Rz_g1

a2 = 20*pi/180
b2 = 30*pi/180
g2 = 30*pi/180
R2 = Rx_a2*Ry_b2*Rz_g2

Decomposing the individual matrices R1 and R2 results in the rights angles. The problem occures, when I link the rotations after each other and I try to determinate the angles of the last frame in the inertial frame. Theoretically this could be done by decomposing the product of all rotational matrices of the chain of transformations.