先看书了解一下等效转轴与等效转角
下面是笔记记录部分
像确定四元数一样，确定轴和角度也只能直到符号为止。即，（u，θ）和（-u，-θ）对应于相同的旋转矩阵，就像q和-q一样。此外，轴角提取还有其他困难。角度可以限制为0°至180°，但是角度在形式上是360°的倍数不明确。当角度为零时，轴是不确定的。当角度为180°时，矩阵变得对称，这对提取轴有影响。在接近180°的倍数时，需要注意避免数值问题：在提取角度时，atan2（sinθ，cosθ）等于θ的两参数反正切可避免arccos的不敏感性；在计算轴的大小以强制单位大小时，蛮力方法会由于下溢而失去准确性（Moler＆Morrison 1983）。
轴角满足右手螺旋定则，用来确定旋转轴的方向。轴角转旋转矩阵很简单，直接抄书本上的公式即可。下面给个matlab函数参考：
function R=Av2R(theta,k)
kx = k(1); ky = k(2); kz = k(3);
vq = 1 - cos(theta);
sq = sin(theta);
cq = cos(theta);
R = [kx*kx*vq+cq ky*kx*vq-kz*sq kz*kx*vq+ky*sq;
kx*ky*vq+kz*sq ky*ky*vq+cq kz*ky*vq-kx*sq;
kx*kz*vq-ky*sq ky*kz*vq+kx*sq kz*kz*vq+cq;];
end
旋转矩阵转轴角
但是旋转矩阵转轴角就稍微有点麻烦了，需要讨论一下。如果直接按照书本上的公式去计算，当theta=0或者180度时，sin(theta)=0，不能作为分母，直接按照公式算就是错的。Theta=0或者pi时，有一个共同的特点，就是算出来的旋转矩阵R是对称的，因此可以作为判断0或者pi的入口条件。 现在能够已知的就是，当theta=0时，那就是没有动，所以R就是单位阵[1,0,0;0,1,0;0,0,1]，主对角线元素相加等于3，对称元素相等，此时theta就确定=0，轴的话就是任意的，因为绕着任何轴转0度都能产生相同的效果，那就是不动。此时可以设定theta=0时轴k=[1,0,0]。 如果不等于0度，那就说明theta=180度。 当theta=180度时，上面程序中 cq=cos(180°)=-1，sq=sin(180°)=0，vq=2，因此可以得到
R = [2*x*x-1 2*x*y 2*x*z;
2*x*y 2*y*y-1 2*y*z;
2*x*z 2*y*z 2*z*z-1].
根据上面的公式，很容易能够得到x=-+sqrt((r11+1)/2)，y=-+sqrt((r22+1)/2)，z=-+sqrt((r33+1)/2)，因此如何选择正负成了问题。为了解决这个问题，我们仅使用对角线之一，并计算与之相对的其他项（使用元素所在这一列）。这意味着我们在计算的第一项中选择正数还是负数都没有关系。因为现在正在处理的特定情况是旋转180°，该旋转与-180°相同，反转角度与反转轴相同，因此如果x，y，z是我们想要的轴，那么-x，-y，-z也有效。因此，只有这些项的相对符号才重要，我们可以从矩阵的非对角项中获得这些项。 如果要用上面公式计算x，y或者z的话，就必须保证开方里面大于0，为了保证这一点，我们比较主对角线的项，选最大值，并只使用包含最大对角线项的那一列数据，例如： 得到最大对角线项为2xx-1，那么 x=sqrt((r11+1)/2) y=(r21+r12)/4 z=(r31+r13)/4 为了使舍入误差最小，使用对称项的平均值，因此就有y=(r21+r12)/4，z=(r31+r13)/4。 参考函数：
function [theta,n]=R2Av(R)
r11=R(1,1);r12=R(1,2);r13=R(1,3);r21=R(2,1);r22=R(2,2);r23=R(2,3);
r31=R(3,1);r32=R(3,2);r33=R(3,3);
epsilon = 0.0001;
% % 判断矩阵是否对称，如果对称就说明找到0或者pi
if abs(r21 - r12) < epsilon && abs(r31 - r13) < epsilon && abs(r32 - r23) < epsilon % 判断对称
% 0对应的R很简单，[1 0 0; 0 1 0; 0 0 1]
if abs(r21 + r12) < epsilon && abs(r31 + r13) < epsilon && abs(r32 + r23) < epsilon ...
&& abs(r11 + r22 + r33 - 3) < epsilon
% 肯定是0
theta = 0;
n = [1 0 0];
else
% 这里就是pi
theta = pi;
xx = (r11 + 1) / 2;
yy = (r22 + 1) / 2;
zz = (r33 + 1) / 2;
xy = (r12 + r21) / 4;
xz = (r13 + r31) / 4;
yz = (r32 + r23) / 4;
% 找对角线上元素最大者，按最大者所在列进行计算
if (xx > yy) && (xx > zz)
if xx < epsilon
x = 0;
y = 0.7071;
z = 0.7071;
else
x = sqrt(xx);
y = xy / x;
z = xz / x;
end
elseif (yy > zz) && (yy > xx)
if yy < epsilon
y = 0;
x = 0.7071;
z = 0.7071;
else
y = sqrt(yy);
x = xy / y;
z = yz / y;
end
else
if zz < epsilon
z = 0;
x = 0.7071;
y = 0.7071;
else
z = sqrt(zz);
x = xz / z;
y = yz / z;
end
end
n = [x y z];
end
else
theta=acos((r11+r22+r33-1)/2);
k=1/(2*sin(theta))*[r32-r23;r13-r31;r21-r12];
n=k';
end
end
9.10和10.3工具箱中该函数的区别
测试发现，Peter工具箱9.10版本中的tr2angvec函数每次得到的轴方向都是一样的，意思就是按右手定则，从姿态R1转到R2，算出来的delta_R1计算出的轴角[theta, k1]，和，同样按照右手定则，从姿态R2反转到R1，算出来的delta_R2计算出的轴角[theta, k2]一模一样。theta一样当然是对的，但是你的轴不能一模一样啊，反转之后，轴也应该是反向的。。。10.3就不会，所以要用的话，直接用10.3里的函数吧。我这里把函数贴出来。
%TR2ANGVEC Convert rotation matrix to angle-vector form
%
% [THETA,V] = TR2ANGVEC(R, OPTIONS) is rotation expressed in terms of an
% angle THETA (1x1) about the axis V (1x3) equivalent to the orthonormal rotation
% matrix R (3x3).
%
% [THETA,V] = TR2ANGVEC(T, OPTIONS) as above but uses the rotational part of the
% homogeneous transform T (4x4).
%
% If R (3x3xK) or T (4x4xK) represent a sequence then THETA (Kx1)is a vector
% of angles for corresponding elements of the sequence and V (Kx3) are the
% corresponding axes, one per row.
%
% Options::
% 'deg' Return angle in degrees
%
% Notes::
% - For an identity rotation matrix both THETA and V are set to zero.
% - The rotation angle is always in the interval [0 pi], negative rotation
% is handled by inverting the direction of the rotation axis.
% - If no output arguments are specified the result is displayed.
%
% See also ANGVEC2R, ANGVEC2TR, TRLOG.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function [theta_, n_] = tr2angvec(R, varargin)
assert(ishomog(R) || isrot(R), 'RTB:tr2angvec:badarg', 'argument must be SO(3) or SE(3)');
opt.deg = false;
opt = tb_optparse(opt, varargin);
% get the rotation submatrix(s)
if ~isrot(R)
R = t2r(R);
end
if size(R,3) > 1
theta = zeros(size(R,3),1);
v = zeros(size(R,3),3);
end
for i=1:size(R,3) % for each rotation matrix in the sequence
% There are a few ways to do this:
%
% 1.
%
% e = 0.5*vex(R - R'); % R-R' is skew symmetric
% theta = asin(norm(e));
% n = unit(e);
%
% but this fails for rotations > pi/2
%
% 2.
%
% e = vex(logm(R));
% theta = norm(e);
% n = unit(e);
%
% elegant, but 40x slower than using eig
%
% 3.
%
% Use eigenvectors, get angle from trace which is defined over -pi to
% pi. Don't use eigenvalues since they only give angles -pi/2 to pi/2.
%
% 4.
%
% Take the log of the rotation matrix
Ri = R(:,:,i);
% check the determinant
assert( abs(det(Ri)-1) < 10*eps, 'RTB:tr2angvec:badarg', 'matrix is not orthonormal');
[th,v] = trlog(Ri);
theta(i) = th;
n(i,:) = v;
if opt.deg
theta(i) = theta(i) * 180/pi;
units = 'deg';
else
units = 'rad';
end
if nargout == 0
% if no output arguments display the angle and vector
fprintf('Rotation: %f %s x [%f %f %f]\n', theta(i), units, n(i,:));
end
end
if nargout == 1
theta_ = theta;
elseif nargout == 2
theta_ = theta;
n_ = n;
end
%TRLOG logarithm of SO(3) or SE(3) matrix
%
% S = trlog(R) is the matrix logarithm (3x3) of R (3x3) which is a skew
% symmetric matrix corresponding to the vector theta*w where theta is the
% rotation angle and w (3x1) is a unit-vector indicating the rotation axis.
%
% [theta,w] = trlog(R) as above but returns directly theta the rotation
% angle and w (3x1) the unit-vector indicating the rotation axis.
%
% S = trlog(T) is the matrix logarithm (4x4) of T (4x4) which has a (3x3)
% skew symmetric matrix upper left submatrix corresponding to the vector
% theta*w where theta is the rotation angle and w (3x1) is a unit-vector
% indicating the rotation axis, and a translation component.
%
% [theta,twist] = trlog(T) as above but returns directly theta the rotation
% angle and a twist vector (6x1) comprising [v w].
%
% Notes::
% - Efficient closed-form solution of the matrix logarithm for arguments that are
% SO(3) or SE(3).
% - Special cases of rotation by odd multiples of pi are handled.
% - Angle is always in the interval [0,pi].
%
% References::
% - "Mechanics, planning and control"
% Park & Lynch, Cambridge, 2016.
%
% See also trexp, trexp2, Twist.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function [o1,o2] = trlog(T)
if isrot(T)
% deal with rotation matrix
% closed form solution for matrix logarithm of a homogeneous transformation (Park & Lynch)
% that handles the special cases
% for now assumes T0 is the world frame
R = T;
if abs(trace(R) - 3) < 100*eps
% matrix is identity
w = [0 0 0]';
theta = 0;
elseif abs(trace(R) + 1) < 100*eps
% tr R = -1
% rotation by +/- pi, +/- 3pi etc
[mx,k] = max(diag(R));
I = eye(3,3);
col = R(:,k) + I(:,k);
w = col / sqrt(2*(1+mx));
theta = pi;
% skw = logm(R);
% w = vex( skw );
else
% general case
theta = acos( (trace(R)-1)/2 );
skw = (R-R')/2/sin(theta);
w = vex(skw); % is a unit vector
end
if nargout <= 1
o1 = skew(w*theta);
elseif nargout ==2
o1 = theta;
o2 = w;
end
elseif ishomog(T)
% SE(3) matrix
[R,t] = tr2rt(T);
if abs(trace(R) - 3) < 100*eps
% rotation matrix is identity, theta=0
w = [0 0 0]';
v = t;
theta = 1;
skw = zeros(3,3);
else
[theta, w] = trlog(R);
skw = skew(w);
Ginv = eye(3,3)/theta - skw/2 + ( 1/theta - cot(theta/2)/2 )*skw^2;
v = Ginv * t;
end
if nargout <= 1
o1 = [skw v; 0 0 0 0]*theta;
elseif nargout ==2
o1 = theta;
o2 = [v; w];
end
else
error('RTB:trlog:badarg', 'expect SO(3) or SE(3) matrix');
end
end
% [th,w] = tr2angvec(R);
% w = w'
%
% d = dot(unit(w), transl(T))
% h = d / th
%
% q = (transl(T) - h*th*w ) * inv(eye(3,3) - R)
%
% v =
% rho = (eye(3,3) - R')*t / 2 / (1-cos(th));
%
% v = cross(rho, w);
%
% tw = [skew(unit(w)) v'; 0 0 0 0];
如有纰漏，欢迎批评指正！
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明机器人学回炉重造（1-3）：旋转矩阵转轴角、轴角转旋转矩阵！
