Difference between 2 orientations
显示 更早的评论
I have a sensor in an orientation - let's call it the target orientation. The orientation is in euler angles in degrees (pitch = 50, roll = 50 and yaw = 50). I move the sensor about then I try to return it to the target orientation. The 'attempt' orientation is in euler angles (pitch = 70, roll = 70 and yaw = 70). I am interested in the difference between the 2 orientations (how close is the 'attempt' to the 'target' orientation?).
3 suggested methods have been proposed:
- Subtract the orientations to yield a difference of 20 degrees pitch, 20 degrees roll and 20 degrees yaw.
- Compute the resultant orientation between the 2 orientations by converting euler angles to rotation matrices, multipling 2 rotation matrices and then extracting the euler angles (this gives 19 degrees pitch, 12 degrees roll and 4 degrees yaw.
- Converting the euler angles to quaternions and multipling the 2 qw components, then extracting an rotation from this (this gives 105 degrees).
I am interested in the 'correct way' are reporting the error between the attempt and the target. Any help gratefully recieved.
I post the code for clarity. Notes for the code - euler angles with an a-suffix are the target orientation, b-suffix are the attempt orientation. Is it written with for loops for when time series data are added.
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
ch = cos(hdga); sh = sin(hdga);
cp = cos(pitcha); sp = sin(pitcha);
cr = cos(rolla); sr = sin(rolla);
qxa = (sin(rolla*0.5) * cos(pitcha*0.5) * cos(hdga*0.5)) - (cos(rolla*0.5) * sin(pitcha*0.5) *sin(hdga*0.5));
qya = (cos(rolla*0.5) * sin(pitcha*0.5) * cos(hdga*0.5)) + (sin(rolla*0.5) * cos(pitcha*0.5) * sin(hdga*0.5));
qza = (cos(rolla*0.5) * cos(pitcha*0.5) * sin(hdga*0.5)) - (sin(rolla*0.5) * sin(pitcha*0.5) * cos(hdga*0.5));
qwa = (cos(rolla*0.5) * cos(pitcha*0.5) * cos(hdga*0.5)) + (sin(rolla*0.5) * sin(pitcha*0.5) * sin(hdga*0.5));
len = length(hdga);
Rrpha = zeros(len,9);
Rrpha(:,1:3) = [cp.*ch, -sp, cp.*sh];
Rrpha(:,4:6) = [cr.*sp.*ch + -sr.*-sh, cr.*cp, cr.*sp.*sh + -sr.*ch];
Rrpha(:,7:9) = [sr.*sp.*ch + cr.*-sh, sr.*cp, sr.*sp.*sh + cr.*ch];
%%
ch = cos(hdgb); sh = sin(hdgb);
cp = cos(pitchb); sp = sin(pitchb);
cr = cos(rollb); sr = sin(rollb);
qxb = (sin(rollb*0.5) * cos(pitchb*0.5) * cos(hdgb*0.5)) - (cos(rollb*0.5) * sin(pitchb*0.5) *sin(hdgb*0.5));
qyb = (cos(rollb*0.5) * sin(pitchb*0.5) * cos(hdgb*0.5)) + (sin(rollb*0.5) * cos(pitchb*0.5) * sin(hdgb*0.5));
qzb = (cos(rollb*0.5) * cos(pitchb*0.5) * sin(hdgb*0.5)) - (sin(rollb*0.5) * sin(pitchb*0.5) * cos(hdgb*0.5));
qwb = (cos(rollb*0.5) * cos(pitchb*0.5) * cos(hdgb*0.5)) + (sin(rollb*0.5) * sin(pitchb*0.5) * sin(hdgb*0.5));
Rrphb = zeros(len,9);
Rrphb(:,1:3) = [cp.*ch, -sp, cp.*sh];
Rrphb(:,4:6) = [cr.*sp.*ch + -sr.*-sh, cr.*cp, cr.*sp.*sh + -sr.*ch];
Rrphb(:,7:9) = [sr.*sp.*ch + cr.*-sh, sr.*cp, sr.*sp.*sh + cr.*ch];
for n= 1: len
dca{n}=[Rrpha(n,1) Rrpha(n,2) Rrpha(n,3); Rrpha(n,4) Rrpha(n,5) Rrpha(n,6); Rrpha(n,7) Rrpha(n,8) Rrpha(n,9)];
end
for n= 1: len
dcb{n}=[Rrphb(n,1) Rrphb(n,2) Rrphb(n,3); Rrphb(n,4) Rrphb(n,5) Rrphb(n,6); Rrphb(n,7) Rrphb(n,8) Rrphb(n,9)];
end
for n= 1: len
R{n}=(dca{n}'* dcb{n});
roll(n) = (atan2(R{n}(3,2),R{n}(2,2)))/pi*180
heading(n) = (atan2(R{n}(3,1),R{n}(1,1)))/pi*180
pitch(n) = -asin(R{n}(1,2))/pi*180
end
qwR = qwa*qwb;
qwRES = rad2deg(2*acos(qwR))
12 个评论
Jonathan Williams
2023-7-13
Bruno Luong
2023-7-13
To illustrate option 1 is not fully correct is similar to the siluation where you find the distance between 2 points on the earth. If you have longitude and latitude of both points, it is tempting to make the difference of longitudes and latitudes then somehow combine them as "distance". This is NOT correct since the true distance is the separation on great circle passing through both points.
Why it is not correct? I believe the triangular inequality is not respected if you do the first method for point closes to the poles. So it NOT a distances. Stuffs like that will end up having non robust code if you don't use correct mathematical definition with proper properties.
Jonathan Williams
2023-7-17
Paul
2023-7-17
Is it possible to deduce one has moved further by 10 degrees in each plane,
Let's start with two arms perfectly aligned with each other. To make it simple, we will assume that the coordinate axes are fixed to each arm are aligned with our world axes.
The first arm rotates through a 30 deg roll and the second arm rotates through a 50 deg roll. These two angles are diretcly comparable becasue they are both rotations around the same axes, namely the x-axis of the world frame (which happens to also be the x-axis of each arm frame). So a 20 deg difference. Now, apply a pitch rotation to each arm of 30 degrees. These rotations are not comparable, because they are each a rotation around a different axis. Arm-1 is rotating about its y-axis and arm-2 is rotating about its y-axis. But these axes aren't aligned with each other because the preceding roll angles were different, so the pitch rotations represent different motion when viewed relative to the world (try to picture what happens on the pitch rotations when arm-1 roll is 0 and arm-2 roll is 90). And then the same issue with the yaw rotation. Basically, unless the rotations are around the same axis, they can't be subtracted from (or added to) each other.
Maybe your really asking this question ...
Define the DCM from the world frame to the arm-1 frame:
world2arm1 = angle2dcm(50*pi/180,40*pi/180,30*pi/180,'XYZ');
And similarly for arm-2
world2arm1 = angle2dcm(60*pi/180,60*pi/180,60*pi/180,'XYZ');
Do you want then determine the delta angles relative to (50,40,30) that result in the arm-2 orientation, i.e., solve this equation for deltar, deltap, deltay?
% world2arm2 == angle2dcm((50+deltar)*pi/180,(40+deltay)*pi/180,(30+detay)*pi/180,'XYZ');
In theory, this might be doable, but I'm not sure how meaningful the result would be.
Bruno Luong
2023-7-17
@Jonathan Williams It is just me but but I read through your many questions in the last comment few times, I still don't understand what you are asking.
Jonathan Williams
2023-7-18
James Tursa
2023-7-18
编辑：James Tursa
2023-7-18
3D rotations in general are never directly comparible with simple subtractions of the Euler angles, as shown in the answers below. The only time you could do that calculation and have it make sense is if the angles in question are only planar (motion taking place all in one plane), or are very small are you are doing a very small angle approximation to the rotations. But that is not what you are doing.
Bruno Luong
2023-7-19
编辑：Bruno Luong
2023-7-19
Further more euler angles are not uniquely defined for a given orientation, generally there is 2 solutions (not counting the fact that each angle can be wrapped independently on 360 degrees)
When comparing 2 orientations so between 2 x 2 sets of euler angles, which difference (out of 4 possibility) is "right" ?
Topogically the orientation space SO(3) is different than Euler-angles which is SO(1)^3. Therefore it must have a discontinuity when mapping orientation to Euler angles, meaning it must have two orientation arbitrary close BUT their respective Euler angles are not close to each other (this point is like the two poles on earch which does not have continuity of lon/lat coordinates).
That's why Euler angles cannot be used consistently for any calculation of orientation.
To illustrate the discontinuity of euler angles with a concrete numerical example, see the result of the bellow code. So clearly why it should NOT be used to evaluate the difference between two orientations:
% Generate the first orientation
A = [1 0 0;
0 1 0;
1e-12 1e-6 1];
% Orthonormalize
[A,~] = qr(A);
% Make sure axis direction are as I wanted (close to Identity)
for k=1:3
if A(k,k)<0
A(:,k) = -A(:,k);
end
end
A
[PhiA, ThetaA, PsiA] = RotMat3Eu(A)
% Generate the second orientation
B = [1 0 0;
0 1 0;
-1e-12 -1e-6 1];
[B,~] = qr(B);
for k=1:3
if B(k,k)<0
B(:,k) = -B(:,k);
end
end
B
[PhiB, ThetaB, PsiB] = RotMat3Eu(B)
% small showing A ~ B
norm(A-B)
% geodesic distance SO(3), David's method
lbd = eig(B'*A);
dAB = angle(lbd(find(imag(lbd)~=0,1)))
% Difference two of the Euler angles (Phi and Psi), ~ PI
% so the delta does NOT reflect the fact that A ~ B
dEul = [PhiA, ThetaA, PsiA] - [PhiB, ThetaB, PsiB]
function [Phi, Theta, Psi] = RotMat3Eu(A)
% Compute Euler angles from rotation matrix
% https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
% Rotation matrix → Euler angles (z-x-z extrinsic), just to illustrate for
% this specific convention, but the same phenomenon occurs for ANY convention
Phi = atan2(A(3,1),A(3,2));
Theta = acos(A(3,3));
Psi = -atan2(A(1,3),A(2,3)); % EDIT after Paul spots the sign error
end
Paul
2023-7-19
Shouldn't the equation for Psi be (with negative sign):
Psi = -atan2(A(1,3),A(2,3));
Bruno Luong
2023-7-19
编辑：Bruno Luong
2023-7-19
David Goodmanson
2023-7-20
编辑：David Goodmanson
2023-8-4
Hi Jonathan,
Comparing rotation matrices A and B, each expressed by three euler angles, can be tricky as you can see. That's why, instead of comparing two sets of three euler angles I like the idea of expressing B\A as a single rotation about some particular axis, with the angle of rotation (or a function of it such as the energy-like 8*sin(theta/2)^2 ) expressing the disagreement between A and B. It doesn't tell you how to further rotate B to make it agree with A, but at least it's a single consistent criterion for the difference between them.
回答（3 个）
I would do your method #3 involving quaternions. This would give you the "smallest" rotation between the two attitudes. But you need to multiply the inverse of one times the other, not just a simple multiply (this is analagous to transposing one of your rotation matrices). It isn't clear what the intended order of these rotations are, so you may have to adjust the code below, but just using 'XYZ' order for example gives:
% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
% Using quaternions
qa = eul2quat([rolla,pitcha,hdga],'XYZ');
qb = eul2quat([rollb,pitchb,hdgb],'XYZ');
qr = quatmultiply(qa,quatinv(qb));
if( qr(1) < 0 )
qr = -qr;
end
angle_quat = 2*acosd(qr(1))
% Using rotation matrices
ma = eul2rotm([rolla,pitcha,hdga],'XYZ');
mb = eul2rotm([rollb,pitchb,hdgb],'XYZ');
mr = inv(mb) * ma;
q = rotm2quat(mr);
if( q(1) < 0 )
q = -q;
end
angle_rotm = 2*acosd(q(1))
If you don't have the Aerospace Toolbox, then you would have to write your own low level code for these conversions as you have done in your original post.
5 个评论
Paul
2023-7-12
Any particular reason to prefer
inv(mb) * ma % (1)
instead of
mb \ ma % (2)
if wanting to use the inverse,
or using the inverse property of rotation matrices?
mb.' * ma % (3)
In this particular instance it makes no difference, just asking if you prefer (1) over (2) or (3) for some technical or philosophical reason.
James Tursa
2023-7-12
编辑：James Tursa
2023-7-12
The only reason was to make it look similar to the quaternion code for comparison (you see the 'inv' in both). Nothing else. Of course all your points are correct, and I could have used conjugate for the quaternion and transpose for the rotation matrix and got the same result. And my code above assumes the quaternions and rotation matrices produced by the MATLAB functions are proper so no checks or adjustments are made. But if the quaternions or rotation matrices came from some other source one may want to perform checks.
And, after thinking more about this in terms of coordinate system transformations, switching which variables gets the inv( ) might make more sense to keep the coordinate systems consistent. E.g., supposing intertial and body frames:
% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
% Using quaternions
qa = eul2quat([rolla,pitcha,hdga],'XYZ'); % INERTIAL -> BODY1
qb = eul2quat([rollb,pitchb,hdgb],'XYZ'); % INERTIAL -> BODY2
qr = quatmultiply(quatconj(qa),qb); % B1->I * I->B2 = BODY1 -> BODY2
if( qr(1) < 0 )
qr = -qr;
end
angle_quat = 2*acosd(qr(1))
angle_quat = 43.4416
% Using rotation matrices
ma = eul2rotm([rolla,pitcha,hdga],'XYZ'); % INERTIAL -> BODY1
mb = eul2rotm([rollb,pitchb,hdgb],'XYZ'); % INERTIAL -> BODY2
mr = mb * ma'; % I->B2 * B1->I = BODY1 -> BODY2
q = rotm2quat(mr);
if( q(1) < 0 )
q = -q;
end
angle_rotm = 2*acosd(q(1))
I understand the sentiment, but eul2rotm is defined such that its output is premultiplied by a row vector. So if we a have a row vector resolved in inertial coordinates, that same vector resolved in body1 and body2 coordinates would be
xrow_body1 = xrow_inertial*ma
xrow_body2 = xrow_inertial*mb
Combining
xrow_body2 = xrow_body1*tanspose(ma)*mb
which was the original solution.
If using the Aerospace toolbox function angle2dcm it would go the other way because that function returns the DCM to post-multiply by a column vector resolved in inertial coordinates to get that same vector resolved in body coordinates.
@Paul Yikes! I rarely use Euler Angles myself so I missed that in the doc. It's hard to keep up with the different conventions in the Aerospace, Navigation, and Robotics toolboxes, and the generic quaternion( ) class. To your point ...
r = deg2rad(20); p = deg2rad(40); y = deg2rad(60); eul = [r p y];
eul2rotm(eul,'XYZ')
angle2dcm(r,p,y,'XYZ')
David Goodmanson
2023-7-12
编辑：David Goodmanson
2023-7-20
Hello Jonathan,
You have two 3x3 rotation matrices, each expressed in terms of euler angles,
Rrpha(:,1:3) = [cp.*ch, -sp, cp.*sh];
Rrpha(:,4:6) = [cr.*sp.*ch + -sr.*-sh, cr.*cp, cr.*sp.*sh + -sr.*ch];
Rrpha(:,7:9) = [sr.*sp.*ch + cr.*-sh, sr.*cp, sr.*sp.*sh + cr.*ch];
and similarly for Rrphb. I will call these A and B for short. Ideally these two matrices equal each other, so
inv(B)*A = B\A = I, and ideally (B\A -I) = 0
In actuality, D = (B\A-I) is not zero, and
D = B\A -eye(3);
then E = sum(D.*D,'all')
is a reasonable measure of the difference between A and B. You can express that quantity algebraically in terms of a bunch of euler angles for A and B, but since you are looking for a numerical result it seems easiest to just calculate A and B with the known euler angles and then find E.
If you just need a measure of the difference between A and B, and you don't need the rotation matrix B\A in terms of more euler angles, then I don't see the need to convert A or B to quaternions. If you do need B\A in terms of angles, quaternions are a good way to go, but for getting B\A in terms of euler angles one way is by the code down at the end.
Every 3d rotation can be expressed in terms of rotation by an angle theta about a particular unit vector w in 3d space. It can be shown that the quantity E is simply
E = 8*sin(theta/2)^2
regardless of the direction of w. So calculation of E can give you the single euler-type rotation angle, within a sign. The smaller the angle, the closer A is to B.
The quantity E has a geometric interpretation. Let
D = [a b c
d e f
g h k]
If ux is the unit vector in the x direction, ux = [1 0 0]'
then D*ux = [a d g]'
and the squared distance between ux and D*ux is (a-1)^2 + d^2 + g^2 which is three of the terms making up E. Similarly for uy and uz so overall you get the sum of the squared distances between what you have and what you would like to get.
You can find w and theta, within a sign, by looking at
[v lam] = eig(B\A)
As long as B\A is not the identity, you should get one eigenvalue = 1 and two eigenalues that are complex conjugates on the unit circle, exp(i*theta) and exp(-i*theta). Theta is the rotation angle, same one as determined by E.
The column of v that corresponds to eigenvalue 1 is the rotation axis w. [ eig probably produces a vector w that has real components, but there is no guarantee of that. w might possibly be multiplied by an overall phase constant exp(i*phi). To get rid of it, you can do
[~,ind] = max(abs(w))
phi = angle(w(ind))
w = real(w*exp(-i*phi)) ]
There are two eigenvectors corresponding to the eigenvalues exp(+-i*theta), and those eigenvectors are complex conjugates of each other. Taking one of them and denoting it by v1, then let
w1 = real(v1), w2 = imag(v1)
and w, w1, w2 are a real orthonormal set of vectors. The rotation is about w, in the w1,w2 plane,
To express B\A in terms of euler angles:
function theta = anglesR(R,str)
% solve a 3d rotation matrix R in the form Ra(th1)*Rb(th2)*Rc(th3)
% for angles th1,th2,th3.
% str is a three-letter input string such as 'xyz'
% that provides the rotation axes a,b,c respectively.
% consecutive rotations along the same axis such as 'xxy' are not allowed.
% 1st and 3rd rotations along different axes are tait-bryan,
% along the same axis are euler. 3x2x2 = 12 possibilities in all.
% Output is the vector [th1 th2 th3] and angles are in DEGREES, with
% -180<(th1,th3)<180 -90<th2<90 (tait-bryan), 0<th2<180 (euler)
%
% theta = anglesR(R,str)
% David Goodmanson
theta = zeros(1,3);
% useful similarity transform matrices
By = [0 0 1;0 -1 0;1 0 0]; % x<-->z y(th)-->y(-th)
Ry90 = [0 0 1;0 1 0; -1 0 0]; % y rotation by 90 deg
C = [0 0 1;1 0 0; 0 1 0;]; % cycic, x-->y-->z-->x
signy = 1;
% transform to RdRyRe
if str(2) == 'x'
R = C*R/C;
elseif str(2) == 'z'
R = C\R*C;
end
% tait-bryan, transform to RxRyRz
if all(str=='xzy')|all(str=='yxz')|all(str=='zyx') % transformed str is 'zyx'
R = By*R/By;
signy = -1;
end
% euler, transform to RxRyRx
if all(str=='xzx')|all(str=='yxy')|all(str=='zyz') % transformed str is 'zyz'
R = Ry90*R/Ry90;
end
if str(1)~=str(3) % tait-bryan RxRyRz -90 <= th2 <= 90
theta(2) = signy*asind(R(1,3));
theta(1) = atan2d(-R(2,3),R(3,3));
theta(3) = atan2d(-R(1,2),R(1,1));
else % euler RxRyRx 0 <= th2 <= 180
theta(2) = acosd(R(1,1));
theta(1) = atan2d(R(2,1),-R(3,1));
theta(3) = atan2d(R(1,2), R(1,3));
end
end
% Rotation of an object's coordinates.
% ccw rotation looking down at rotation axis coming up out of page.
%
% [1 0 0 [c 0 s [c -s 0
% Rx = 0 c -s Ry = 0 1 0 Rz = s c 0
% 0 s c] -s 0 c] 0 0 1]
%
% tait-bryan RxRyRz (elements represented by . are more complicated)
% [c2c3 -c2s3 s2
% . . -s1c2
% . . c1c2]
%
% euler RxRyRx
% [c2 s2s3 s2c3
% s1s2 . .
% -c1s2 . . ]
I recall a long discussion with Roger, James on this topic on the newsgroup long time ago.
At the time our conclusion is quaternion is the best, thank you James. The EIG method as David's solution is not accurate numerically in some extrem cases.
Since then I have tested several other methods of computing the rotation vector (rotation angle * unit vector of a rotation) and the Cayley method beats even the quaternion in term of robustness.
Using James's example
% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
% Using rotation matrices
Q1 = eul2rotm([rolla,pitcha,hdga],'XYZ');
Q2 = eul2rotm([rollb,pitchb,hdgb],'XYZ');
For two frames Q1 and Q2, call
[theta, u] = CayleyMethod(Q1*Q2');
rad2deg(theta)
will give theta the rotation angle (in radian); one can consider abs(theta) as the "distance" between Q1 and Q2, since Q1 can obtained by rotation of Q2 about u of +/-abs(theta). One can prove this is most economical rotation to map Q1 to Q2 (and vice versa) since abs(theta) is minimal.
%%
function [theta, u] = CayleyMethod(A)
% Ref: "A Survey on the Computation of Quaternions from Rotation Matrices",
% Soheil Sarabandi, Federico Thomas
% J. Mechanisms Robotics. Apr 2019
p = size(A,3);
R = reshape(A,9,p);
C = [R(6,:)-R(8,:);
R(7,:)-R(3,:);
R(2,:)-R(4,:)];
D = [R(6,:)+R(8,:);
R(7,:)+R(3,:);
R(2,:)+R(4,:)];
C2 = C.^2;
D2 = D.^2;
E0 = (R(1,:)+R(5,:)+R(9,:)+1).^2 + C2(1,:) + C2(2,:) + C2(3,:);
Exyz2 = [ ...
C2(1,:) + D2(2,:) + D2(3,:) + (R(1,:)-R(5,:)-R(9,:)+1).^2;
C2(2,:) + D2(3,:) + D2(1,:) + (R(5,:)-R(9,:)-R(1,:)+1).^2;
C2(3,:) + D2(1,:) + D2(2,:) + (R(9,:)-R(1,:)-R(5,:)+1).^2
];
c = sqrt(E0);
s = sqrt(sum(Exyz2,1));
theta = 2*atan2(s, c);
u = sign(C).*sqrt(Exyz2)./s;
isId = s == 0;
nId = sum(isId,2);
u(:,isId) = repmat([1; 0; 0], 1, nId);
theta(isId) = 0;
end
类别
在 帮助中心 和 File Exchange 中查找有关 Orientation, Position, and Coordinate Systems 的更多信息
2023-7-12
2023-8-4
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
