1）使用 Rodrigues‘ 旋转公式2）四元数计算旋转矩阵

第一种方式：Rodrigues’ 旋转公式

function R = rotation_matrix(axis, theta)
    % rotation_matrix 计算绕给定轴旋转指定角度的旋转矩阵
    %
    % 参数:
    %   axis - 旋转轴，1x3 向量
    %   theta - 旋转角度，标量（弧度制）
    %
    % 返回:
    %   R - 旋转矩阵，3x3 矩阵

    % 轴单位化
    axis = axis / norm(axis);  
    % 提取轴的分量
    x = axis(1);
    y = axis(2);
    z = axis(3);

    % 计算旋转矩阵
    R = [cos(theta) + x^2 * (1 - cos(theta)), x * y * (1 - cos(theta)) - z * sin(theta), x * z * (1 - cos(theta)) + y * sin(theta);
         y * x * (1 - cos(theta)) + z * sin(theta), cos(theta) + y^2 * (1 - cos(theta)), y * z * (1 - cos(theta)) - x * sin(theta);
         z * x * (1 - cos(theta)) - y * sin(theta), z * y * (1 - cos(theta)) + x * sin(theta), cos(theta) + z^2 * (1 - cos(theta))];
end

详细解释

1. 轴单位化：

   axis = axis / norm(axis);  
   

   将旋转轴向量单位化，以确保旋转轴的长度为1。这是计算旋转矩阵时的标准做法。

2. 提取轴的分量：

   x = axis(1);
   y = axis(2);
   z = axis(3);
   

   将单位化后的旋转轴向量的各个分量（x, y, z）分别提取出来，便于后续计算。

3. 计算旋转矩阵：

   R = [cos(theta) + x^2 * (1 - cos(theta)), x * y * (1 - cos(theta)) - z * sin(theta), x * z * (1 - cos(theta)) + y * sin(theta);
        y * x * (1 - cos(theta)) + z * sin(theta), cos(theta) + y^2 * (1 - cos(theta)), y * z * (1 - cos(theta)) - x * sin(theta);
        z * x * (1 - cos(theta)) - y * sin(theta), z * y * (1 - cos(theta)) + x * sin(theta), cos(theta) + z^2 * (1 - cos(theta))];
   

   使用 Rodrigues’ 旋转公式计算旋转矩阵。公式的每一项均基于旋转角度 θ 以及旋转轴向量的各个分量（x, y, z）进行计算：

   • 主对角线上的元素表示绕轴旋转的余弦部分。
   • 非主对角线上的元素表示旋转轴的分量和正弦、余弦的组合部分。

第二种方式：通过四元数生成旋转矩阵

function r_matrix = rotation_matrix(axis, theta)
    % rotation_matrix 计算绕给定轴旋转指定角度的旋转矩阵
    %
    % 参数:
    %   axis - 旋转轴，1x3 向量
    %   theta - 旋转角度，标量（弧度制）
    %
    % 返回:
    %   r_matrix - 旋转矩阵，3x3 矩阵

    % 将旋转轴变为单位向量
    axis = axis / norm(axis);  
    
    % 计算四元数的实部
    a = cos(theta / 2);
    
    % 计算四元数的虚部
    bcd = - axis .* sin(theta / 2);
    b = bcd(1);
    c = bcd(2);
    d = bcd(3);
    
    % 预计算的变量，用于旋转矩阵的计算
    aa = a^2; 
    bb = b^2; 
    cc = c^2; 
    dd = d^2;
    bc = b * c; 
    ad = a * d; 
    ac = a * c; 
    ab = a * b; 
    bd = b * d; 
    cd = c * d;
    
    % 计算旋转矩阵
    r_matrix = [(aa + bb - cc - dd), (2 * (bc + ad)), (2 * (bd - ac));
                (2 * (bc - ad)), (aa + cc - bb - dd), (2 * (cd + ab));
                (2 * (bd + ac)), (2 * (cd - ab)), (aa + dd - bb - cc)];
end

详细解释

1. 轴单位化：

   axis = axis / norm(axis);  % 将旋转轴变为单位向量
   

   将旋转轴向量单位化，以确保旋转轴的长度为1。

2. 计算四元数的实部：

   a = cos(theta / 2);
   

   计算四元数的实部，等于 cos(theta / 2)。

3. 计算四元数的虚部：

   bcd = - axis .* sin(theta / 2);
   b = bcd(1);
   c = bcd(2);
   d = bcd(3);
   

   • bcd 是四元数的虚部，等于 - axis .* sin(theta / 2)，即旋转轴向量乘以 sin(theta / 2)。
   • b, c, d 分别是四元数虚部的 x, y, z 分量。

4. 预计算的变量：

   aa = a^2; 
   bb = b^2; 
   cc = c^2; 
   dd = d^2;
   bc = b * c; 
   ad = a * d; 
   ac = a * c; 
   ab = a * b; 
   bd = b * d; 
   cd = c * d;
   

   计算旋转矩阵时常用到的中间变量，用于减少重复计算，提高代码的可读性和性能。

5. 计算旋转矩阵：

   r_matrix = [(aa + bb - cc - dd), (2 * (bc + ad)), (2 * (bd - ac));
               (2 * (bc - ad)), (aa + cc - bb - dd), (2 * (cd + ab));
               (2 * (bd + ac)), (2 * (cd - ab)), (aa + dd - bb - cc)];
   

   使用四元数公式计算旋转矩阵。公式的每一项基于四元数的各个分量及其组合：

   • 主对角线上的元素表示四元数的实部和虚部的平方和差。
   • 非主对角线上的元素表示四元数的虚部和实部的乘积组合。

验证等价性

虽然两种方法计算旋转矩阵的数学表达式不同，但它们在功能上是等价的，因为它们都是基于相同的数学原理，且最终结果相同。可以通过以下方式在 MATLAB 中验证这一点：

axis = [1, 0, 0];  % 旋转轴
theta = pi / 4;  % 旋转角度

% 使用 Rodrigues' 公式计算旋转矩阵
R1 = rotation_matrix_rodrigues(axis, theta);

% 使用四元数方法计算旋转矩阵
R2 = rotation_matrix_quaternion(axis, theta);

% 检查两个矩阵是否相同
% 容差值（例如，1e-10）
tolerance = 1e-10;

% 使用容差比较
isEqualWithTolerance = all(abs(R1(:) - R2(:)) < tolerance);

% 显示容差比较结果
if isEqualWithTolerance
    disp('容差比较: 两个旋转矩阵在容差范围内是相等的。');
else
    disp('容差比较: 两个旋转矩阵在容差范围内是不相等的。');
end

在上面的代码中，分别定义了rotation_matrix_rodrigues和rotation_matrix_quaternion函数，这两个函数对应于上述的两种实现方式。

即使两个旋转矩阵 R1 和 R2 在数值上看起来完全相同，有时 ISEQUAL 函数仍可能返回 FALSE。这可能是由于浮点数比较中的微小误差引起的。我们可以使用 ISEQUAL 的近似版本 ISEQUALN 或 ISMEMBER 来进行容差比较，从而解决这一问题。

% 使用 ISEQUAL 比较
isEqual = isequal(R1, R2);

% 显示 ISEQUAL 结果
if isEqual
    disp('ISEQUAL: 两个旋转矩阵是相等的。');
else
    disp('ISEQUAL: 两个旋转矩阵是不相等的。');
end

% 使用 ISEQUALN 比较
isEqualN = isequaln(R1, R2);

% 显示 ISEQUALN 结果
if isEqualN
    disp('ISEQUALN: 两个旋转矩阵是相等的。');
else
    disp('ISEQUALN: 两个旋转矩阵是不相等的。');
end

% 容差值（例如，1e-10）
tolerance = 1e-10;

% 使用容差比较
isEqualWithTolerance = all(abs(R1(:) - R2(:)) < tolerance);

% 显示容差比较结果
if isEqualWithTolerance
    disp('容差比较: 两个旋转矩阵在容差范围内是相等的。');
else
    disp('容差比较: 两个旋转矩阵在容差范围内是不相等的。');
end

% 使用 ISMEMBER 进行容差比较
isEqualWithIsMember = all(ismembertol(R1, R2, tolerance, 'ByRows', true));

% 显示 ISMEMBER 容差比较结果
if isEqualWithIsMember
    disp('ISMEMBER 容差比较: 两个旋转矩阵在容差范围内是相等的。');
else
    disp('ISMEMBER 容差比较: 两个旋转矩阵在容差范围内是不相等的。');
end

通过这种方式，我们可以更精确地验证两个旋转矩阵是否在数值上相等，考虑到浮点数比较中的微小误差。

输出结果

R1 =

    1.0000         0         0
         0    0.7071   -0.7071
         0    0.7071    0.7071


R2 =

    1.0000         0         0
         0    0.7071   -0.7071
         0    0.7071    0.7071

ISEQUAL: 两个旋转矩阵是不相等的。
ISEQUALN: 两个旋转矩阵是不相等的。
容差比较: 两个旋转矩阵在容差范围内是相等的。
ISMEMBER 容差比较: 两个旋转矩阵在容差范围内是相等的。
>>