Difference between 2 orientations

74 次查看（过去 30 天）
Jonathan Williams
Jonathan Williams 2023-7-12
编辑： David Goodmanson 2023-8-4
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:
  1. Subtract the orientations to yield a difference of 20 degrees pitch, 20 degrees roll and 20 degrees yaw.
  2. 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.
  3. 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 个评论
显示 10更早的评论
Bruno Luong
Bruno Luong 2023-7-19
编辑：Bruno Luong 2023-7-19
Yeah good catch @Paul, I correct it. The conclusion is the same.
David Goodmanson
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 个）

James Tursa
James Tursa 2023-7-12
编辑：James Tursa 2023-7-12
Ran in:
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))
angle_quat = 43.4416
% 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))
angle_rotm = 43.4416
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 个评论
显示 3更早的评论
Paul
Paul 2023-7-12
编辑：Paul 2023-7-13
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.
James Tursa
James Tursa 2023-7-14
Ran in:
@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')
ans = 3×3
0.3830 -0.6634 0.6428 0.9237 0.2795 -0.2620 -0.0058 0.6941 0.7198
angle2dcm(r,p,y,'XYZ')
ans = 3×3
0.3830 0.9237 -0.0058 -0.6634 0.2795 0.6941 0.6428 -0.2620 0.7198

请先登录，再进行评论。

David Goodmanson
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 . . ]
Bruno Luong
Bruno Luong 2023-7-12
编辑：Bruno Luong 2023-7-13
Ran in:
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)
ans = 43.4416
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

类别

在 Help Center 和 File Exchange 中查找有关 Coordinate Transformations and Trajectories 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by