三维空间绕任意轴旋转的连续旋转变换矩阵-EW帮帮网

绕任意轴n旋转α角度的变换矩阵除了用罗德里格斯公式求得，也可以用简单的连续旋转的变换矩阵表示

如下图所示n是三维空间中过原点的向量，该向量为旋转的轴线（注意：向量需要先归一化），旋转方向符合右手系。

已知三维空间绕x、y、z轴的旋转矩阵如下（均是正交矩阵）：

$R_{x}(\theta )=\begin{bmatrix} 1& 0&0 \\ 0& cos(\theta) & -sin(\theta)\\ 0&sin(\theta) &cos(\theta) \end{bmatrix}$

$R_{y}(\theta )=\begin{bmatrix} cos(\theta) &0& sin(\theta)\\ 0&1 &0 \\ -sin(\theta) & 0& cos(\theta) \end{bmatrix}$

$R_{z}(\theta )=\begin{bmatrix} cos(\theta) & -sin(\theta)&0 \\ sin(\theta)& cos(\theta) & 0\\ 0&0 &1 \end{bmatrix}$

解决思路：将三维坐标系整体旋转到向量n上，使得z轴与向量n重合，紧接着使用 $R_{z}(\alpha)$ 旋转矩阵，最后将坐标系恢复

如下图所示，首先按z轴旋转，将n转到xOz面内，其旋转角度为n在xOy面上的投影向量与x轴的夹角（注意：顺时针旋转该角度取值范围为[0, 2pi]），nproj是n在xOy面上的投影

则其旋转矩阵“可能”为

$R_{z}(\beta )=\begin{bmatrix} cos(\beta) & -sin(\beta)&0 \\ sin(\beta)& cos(\beta) & 0\\ 0&0 &1 \end{bmatrix}$

$cos(\beta)=\frac{\textit{n}_{proj}\cdot x}{\left \| \textit{n}_{proj} \right \|}$

$sin(\beta)=\frac{\textit{n}_{proj}\times x}{\left \| \textit{n}_{proj} \right \|},if \ n_{projy}<0,then\ sin(\beta)=-sin(\beta)$

为什么是可能呢？应当注意， $R_{z}(\beta)$ 作用时是坐标系和其中的向量一同旋转，而此处的目的是将旋转向量固定，仅旋转坐标轴，下面以二维旋转为例。

#--------------------------------------------------------------题外话--------------------------------------------------------

下图为二维旋转矩阵示意图，当二维旋转矩阵旋转θ角度时，坐标轴连同向量坐标一同发生了变化，原坐标表示为

$\begin{bmatrix} 1 &0 \\ 0&1 \end{bmatrix}\begin{bmatrix} x\\ y \end{bmatrix}$

其旋转后的坐标是

$\begin{bmatrix} cos(\theta) &-sin(\theta) \\ sin(\theta)&cos(\theta) \end{bmatrix}\begin{bmatrix} 1 &0 \\ 0&1 \end{bmatrix}\begin{bmatrix} x\\ y \end{bmatrix}$

旋转后的坐标仍然是用原本未旋转的坐标系表示的，如[1, 1]'旋转45°后坐标为[0, 1.414]，这个结果还是在xy坐标系中，即[1, 1]'旋转到了y轴，其在x'y'坐标系中还是[1, 1]'。那如何保持图中蓝色向量不动，只旋转坐标轴呢？或者说，如何旋转使得图中蓝色向量和x轴在一条直线上呢？

首先，在左边直接乘以旋转矩阵R(θ)的情况下，无论坐标系如何旋转，跟着坐标系一同旋转的向量[a, b]'用新的坐标系x'y'表示依然是[a, b]'，即ax'+by'，而x'和y'分别等于R(θ)[1, 0]'和R(θ)[0, 1]'，那在原来坐标系xy-[1 0; 0 1]中旋转后的向量就是R(θ)[a, b]'。现在就是要求，无论x'y'转到哪里，在xy坐标系中向量的坐标表示形式始终不变，即有

$\begin{bmatrix} 1 &0 \\ 0&1 \end{bmatrix}\begin{bmatrix} {x}\\ {y}\end{bmatrix}=\begin{bmatrix} cos(\theta) &-sin(\theta) \\ sin(\theta)&cos(\theta) \end{bmatrix}\begin{bmatrix} 1 &0 \\ 0&1 \end{bmatrix}\begin{bmatrix} {x}'\\ {y}' \end{bmatrix}$

[x', y']'就是保持向量在原坐标系不动，而在旋转后坐标系中的新坐标，显然有

$\begin{bmatrix} {x}'\\ {y}' \end{bmatrix}=\begin{bmatrix} cos(\theta) &-sin(\theta) \\ sin(\theta)&cos(\theta) \end{bmatrix}^{-1}\begin{bmatrix} x\\ y \end{bmatrix}$

因而，若要保持向量不动，将坐标轴转到某一向量上，其方法就是左乘一个旋转矩阵逆矩阵

#--------------------------------------------------------回到原话题--------------------------------------------------------

综上，上一步的旋转矩阵是错误的，其正确形式应为

${R_{z}(\beta )}^{-1}={\begin{bmatrix} cos(\beta) & -sin(\beta)&0 \\ sin(\beta)& cos(\beta) & 0\\ 0&0 &1 \end{bmatrix}}^{-1}=\begin{bmatrix} cos(\beta) & sin(\beta)&0 \\ -sin(\beta)& cos(\beta) & 0\\ 0&0 &1 \end{bmatrix}$

那下一步，既然n已经转到xOz面内，只需要按y轴旋转将坐标系的z轴转到与n重合即可，图与公式如下所示

${R_{y}(\gamma )}^{-1}={\begin{bmatrix} cos(\gamma) &0 &sin(\gamma) \\ 0&1 &0 \\ -sin{\gamma}& 0 &cos(\gamma) \end{bmatrix}}^{-1}$

$cos(\gamma)=\frac{\textit{n}_{}\cdot z}{\left \| \textit{n}_{} \right \|}$

$sin(\gamma)=\frac{\textit{n}_{}\times z}{\left \| \textit{n}_{} \right \|},if \ n_{x}<0,then\ sin(\gamma)=-sin(\gamma)$

经过一次z轴旋转α角度变换，即绕该向量旋转α，最后通过两次逆矩阵变换，回正坐标系即可，其总的公式如下：

$R(\alpha)=R_{z}(\beta )R_{y}(\gamma )R_{z}(\alpha ){R_{y}(\gamma )}^{-1}{R_{z}(\beta )}^{-1}$

最后用Matlab代码将其结果与罗德里格斯公式对比，其结果一致（比较R和Rstd）

主程序：

%% 验证连续旋转变换矩阵与Rodrigues旋转矩阵
close all
clear
clc

% 初始化
n = Vector3(rand() - 0.5, rand() - 0.5, rand() - 0.5);  % 生成旋转轴向量
alpha = 2 * pi * rand();  % 旋转弧度
xaxis = Vector3(1, 0, 0);
yaxis = Vector3(0, 1, 0);
zaxis = Vector3(0, 0, 1);

%% 连续旋转变换矩阵
n = Vector3(n.x / n.norm, n.y / n.norm, n.z / n.norm);  % 归一化

cosalpha = cos(alpha);
sinalpha = sin(alpha);

if n.isequal(zaxis)  % 与z轴同向
    R = [cosalpha -sinalpha 0; sinalpha cosalpha 0; 0 0 1];
elseif n.isequal(Vector3(0, 0, -1))  % 与z轴反向
    R = [cosalpha -sinalpha 0; sinalpha cosalpha 0; 0 0 1]';
else
    % 计算投影与x轴夹角的正弦余弦
    % n在xOy面的投影->nproj
    nproj = Vector3(n.x, n.y, 0);
    
    nprojdotxaxis = nproj.dot(xaxis);
    nprojcrossxaxis = nproj.cross(xaxis);
    cosbeta = nprojdotxaxis / nproj.norm;
    sinbeta = nprojcrossxaxis.norm / nproj.norm;
    if nproj.y < 0
        sinbeta = -sinbeta;
    end
    
    % 计算向量与z轴夹角的正弦余弦
    ndotzaxis = n.dot(zaxis);
    ncrosszaxis = n.cross(zaxis);
    cosgamma = ndotzaxis / n.norm;
    singamma = ncrosszaxis.norm / n.norm;
    if n.x < 0
        singamma = -singamma;
    end

    Rz = [cosbeta -sinbeta 0; sinbeta cosbeta 0; 0 0 1];
    Ry = [cosgamma 0 singamma; 0 1 0; -singamma 0 cosgamma];
    Rzalpha = [cosalpha -sinalpha 0; sinalpha cosalpha 0; 0 0 1];
    R = Rz * Ry * Rzalpha * Ry' * Rz';
end

%% Rodrigues旋转矩阵
I = eye(3);
Rstd = cosalpha * I + (1 - cosalpha) * ([n.x n.y n.z]' * [n.x n.y n.z]) + sinalpha * [0 -n.z n.y; n.z 0 -n.x; -n.y n.x 0];

创建Vector3类，方便计算

classdef Vector3
    %VECTOR3 构造三维向量，方便计算点积叉积和模运算
    
    properties
        x = 0;
        y = 0;
        z = 0;
    end
    
    methods
        function obj = Vector3(x, y, z)
            %VECTOR3 构造此类的实例
            obj.x = x;
            obj.y = y;
            obj.z = z;
        end
        
        function output = dot(obj, v)
            % 点积运算
            output = obj.x * v.x + obj.y * v.y + obj.z * v.z;
        end
        
        function outputv = cross(obj, v)
            % 叉积运算
            outputv = Vector3(obj.y * v.z - obj.z * v.y, -obj.x * v.z + obj.z * v.x, obj.x * v.y - obj.y * v.x);
        end
        
        function output = norm(obj)
            % 向量的模
            output = sqrt(obj.x ^ 2 + obj.y ^ 2 + obj.z ^ 2);
        end
        
        function output = isequal(obj, v)
            % 向量相等
            if obj.x == v.x && obj.y == v.y && obj.z == v.z
                output = 1;
            else
                output = 0;
            end
        end
    end
end

三维空间绕任意轴旋转的连续旋转变换矩阵

网站公告

今日签到

热门文章

最新发布