机器人学回炉重造(1-3):旋转矩阵转轴角、轴角转旋转矩阵

76
0
2020年11月12日 09时19分

先看书了解一下等效转轴与等效转角

下面是笔记记录部分

像确定四元数一样,确定轴和角度也只能直到符号为止。即,(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];

 

 

如有纰漏,欢迎批评指正!

发表评论

后才能评论