问小白 wenxiaobai
资讯
历史
科技
环境与自然
成长
游戏
财经
文学与艺术
美食
健康
家居
文化
情感
汽车
三农
军事
旅行
运动
教育
生活
星座命理

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

创作时间:
作者:
@小白创作中心

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

引用
CSDN
1.
https://blog.csdn.net/qq_55697246/article/details/142314418

绕任意轴n旋转α角度的变换矩阵除了用罗德里格斯公式求得,也可以用简单的连续旋转的变换矩阵表示。如下图所示n是三维空间中过原点的向量,该向量为旋转的轴线(注意:向量需要先归一化),旋转方向符合右手系。

已知三维空间绕x、y、z轴的旋转矩阵如下(均是正交矩阵):

解决思路:将三维坐标系整体旋转到向量n上,使得z轴与向量n重合,紧接着使用旋转矩阵,最后将坐标系恢复。

如下图所示,首先按z轴旋转,将n转到xOz面内,其旋转角度为n在xOy面上的投影向量与x轴的夹角(注意:顺时针旋转该角度取值范围为[0, 2pi]),nproj是n在xOy面上的投影。则其旋转矩阵“可能”为

为什么是可能呢?应当注意,

作用时是坐标系和其中的向量一同旋转,而此处的目的是将旋转向量固定,仅旋转坐标轴,下面以二维旋转为例。

题外话

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

其旋转后的坐标是

旋转后的坐标仍然是用原本未旋转的坐标系表示的,如[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坐标系中向量的坐标表示形式始终不变,即有

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

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

回到原话题

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

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

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

最后用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
© 2023 北京元石科技有限公司 ◎ 京公网安备 11010802042949号