几种3D 旋转的参数化形式

在 3D 空间中，旋转是 3 自由度的，刚体变换是 6 自由度的（3自由度旋转+3自由度平移）。

3x3 的旋转矩阵有 9 个量，表达了 3自由度的旋转。旋转矩阵是有自约束的，即一个旋转矩阵是一个正交矩阵，而且行列式为1。

如果使用有 4x4（或4x3）个分量的变换矩阵来表示刚体变换，那么相当于用 16 个量（12 个量）来表达 6 自由度，存在很大的信息冗余。

旋转矩阵

信息冗余，有自约束：正交约束，不适合插值

轴角/角轴/旋转向量 Axis-Angle

任意旋转都可以用一个旋转轴和一个旋转角来刻画。

于是可以用一个向量，方向与旋转轴一致，模长等于旋转角，这种向量称之为旋转向量或轴角或角轴。

旋转向量跟旋转矩阵，可以由 Rodrigues's Formula 相互转换。

角轴缺点：旋转轴 v 和角度 theta，与旋转轴 -v，旋转角度 theta±2kpi是一样的。不连续。

欧拉角

把一个旋转分解成 3 次绕不同轴的旋转，使用 3 个分离的转角。

XYZ, ZYZ,ZYX， yaw-pitch-roll，在特定领域，会定义欧拉角的过程。

欧拉角重大缺点是万向锁问题，在中间旋转，比如 pitch 的旋转度数为 90°时，会出现roll 的轴跟 yaw 的轴重叠，这样会缺少一个旋转自由度。

关于万向锁讲解的视频：https://www.youtube.com/watch?v=vLDn-ZITDgA\&t=2s

四元数

既是紧凑的，也没有奇异性。

三维旋转可以由单位四元数描述。

空间中的一个三维点用虚四元数 p 描述，令一个由单位四元数指定的旋转为 q

那么 p 经过旋转之后得到的 p'

p' = qpq^(-1)

q = cos( theta/2 ) +sin( theta/2 )( ui+ vj + w*k )

四元数可以和旋转矩阵和旋转向量相互转换。

四元数缺点： q 跟 -q 表示相同的 3D 旋转（四元数是三维旋转的 double cover"指的是：在四元数的单位球面上，每一个物理旋转对应两点 q 和 −q。这二者在几何效果上无差别，但在数值应用和插值时需要留意）

验证代码1，转化成旋转矩阵

python 复制代码

import numpy as np
from scipy.spatial.transform import Rotation as R

def main():
    # 1. 随机生成一个旋转，并获取相应的四元数
    #    Scipy 的 from_rotvec 可以通过轴-角表示生成一个旋转（此处随机 轴*角）
    random_axis_angle = np.random.randn(3)  # 随机生成一个三维向量
    rot = R.from_rotvec(random_axis_angle)  # 以随机轴向量的范数作为旋转角度
    
    # 从 Rotation 对象获取四元数，scipy 默认顺序为 [x, y, z, w]
    q_scipy = rot.as_quat()
    print("原四元数 (x, y, z, w):", q_scipy)

    # 2. 构造 -q: 只要把四元数取反即可
    #    注意这里依旧要保持同样的顺序 (x, y, z, w)
    neg_q_scipy = -q_scipy
    print("相反四元数 (x, y, z, w):", neg_q_scipy)
    
    # 3. 使用两个 Rotation 对象：q 和 -q
    #    分别对同一个向量进行旋转，看结果是否一致
    #    注意：Rotation.from_quat() 的输入必须是 [x, y, z, w]
    rot_q = R.from_quat(q_scipy)
    rot_neg_q = R.from_quat(neg_q_scipy)
    
    # 4. 测试向量，这里随便选一个或多个
    test_vector = np.array([-1.0, 2.5, 3.7])
    
    rotated_by_q = rot_q.apply(test_vector)
    rotated_by_neg_q = rot_neg_q.apply(test_vector)
    
    print("向量:", test_vector)
    print("使用  q   旋转后的结果:", rotated_by_q)
    print("使用 -q   旋转后的结果:", rotated_by_neg_q)
    
    # 5. 验证是否近似相等
    #    若 q 和 -q 代表同一个 3D 旋转，则结果应当一致（允许一定数值误差）
    if np.allclose(rotated_by_q, rotated_by_neg_q, atol=1e-7):
        print("结论: q 和 -q 对该向量的旋转结果一致，代表同一个旋转。")
    else:
        print("结论: 结果不一致，需检查实现。")

if __name__ == "__main__":
    main()

验证代码2，直接用 qpq^(-1)

python 复制代码

#python -m pip install --upgrade --force-reinstall numpy-quaternion

import numpy as np
import quaternion  # 来自 numpy-quaternion 库

def quater_rotate_vector(q, v):
    """
    用单位四元数 q 旋转三维向量 v (np.array)，
    计算方式：q * (0, v) * q.inverse()
    返回旋转后的向量 (np.array)。
    """
    # 将 v 嵌入成为 '纯四元数' (0, vx, vy, vz)
    p = np.quaternion(0, v[0], v[1], v[2])
    # 旋转: v' = q * p * q.inverse()
    rotated = q * p * q.inverse()
    # 提取旋转结果的 (x, y, z) 部分
    return np.array([rotated.x, rotated.y, rotated.z])

def main():
    # 1. 随机生成一个旋转轴
    axis = np.random.randn(3)
    axis /= np.linalg.norm(axis)  # 归一化
    # 随机生成一个旋转角度
    angle = np.random.rand() * 2.0 * np.pi

    # 2. 构造对应的单位四元数 q
    half_angle = angle / 2.0
    qw = np.cos(half_angle)
    q_xyz = axis * np.sin(half_angle)
    q = np.quaternion(qw, q_xyz[0], q_xyz[1], q_xyz[2])

    # 3. 构造 -q
    neg_q = -q

    # 4. 随机生成一个 3D 向量 p
    p = np.random.randn(3)

    # 5. 分别用 q 和 -q 旋转向量 p
    r1 = quater_rotate_vector(q, p)
    r2 = quater_rotate_vector(neg_q, p)

    print("随机单位四元数 q = ", q)
    print("相反四元数   -q = ", neg_q)
    print("待旋转向量 p = ", p)
    print("\n用  q   旋转 p 的结果 =", r1)
    print("用 -q   旋转 p 的结果 =", r2)
    
    # 6. 验证是否近似相等
    #    对单位四元数而言，q 和 -q 代表同一个旋转，结果应基本相同
    if np.allclose(r1, r2, atol=1e-7):
        print("\n结论: q 和 -q 对该向量的旋转结果一致，代表同一个旋转。")
    else:
        print("\n结论: 结果不一致，需要检查。")

if __name__ == "__main__":
    main()