四元数如何用于 3D 旋转(代替欧拉角和旋转矩阵)【ESP32指向鼠标】

四元数如何用于 3D 旋转(代替欧拉角和旋转矩阵)

在三维空间中,物体的旋转可以用 欧拉角、旋转矩阵 或 四元数 来表示。

四元数相比于欧拉角和旋转矩阵有 计算更高效、避免万向锁、存储占用少 等优点,因此广泛用于 游戏开发、机器人学、计算机图形学和航空航天 等领域。

四元数的定义

一个四元数 q 由四个实数组成:
q = w + x i + y j + z k q=w+xi+yj+zk q=w+xi+yj+zk

其中:w,x,y,z 是实数;i,j,k 是虚单位,满足特定的乘法规则

旋转的基本表示方式

方式 表示方法 优缺点
欧拉角(Euler Angles) (α,β,γ) 对应绕 X, Y, Z 轴的旋转 优点:直观易理解,和现实生活的旋转方式类似。缺点:存在万向锁(Gimbal Lock)问题,计算复杂。
旋转矩阵(Rotation Matrix) 3×3 矩阵 优点:适用于线性代数计算,方便复合旋转。缺点:需要存储 9 个值,数值误差累积会导致非正交性。
四元数(Quaternion) q=w+xi+yj+zk 优点:旋转计算简单,存储更紧凑(只需要 4 个数),避免万向锁,插值平滑。缺点:不直观,不容易手动调整。

旋转四元数的定义

一个 旋转四元数q 表示围绕单位向量 (x,y,z) 旋转角度 θ 的旋转:
q = cos ⁡ θ 2 + sin ⁡ θ 2 ( x i + y j + z k ) q=\cos\frac{\theta}{2}+\sin\frac{\theta}{2}(x\mathbf{i}+y\mathbf{j}+z\mathbf{k}) q=cos2θ+sin2θ(xi+yj+zk)

或写成向量形式:
q = ( cos ⁡ θ 2 , x sin ⁡ θ 2 , y sin ⁡ θ 2 , z sin ⁡ θ 2 ) q=\left(\cos\frac{\theta}{2},x\sin\frac{\theta}{2},y\sin\frac{\theta}{2},z\sin\frac{\theta}{2}\right) q=(cos2θ,xsin2θ,ysin2θ,zsin2θ)

其中:θ 是旋转角度

(x,y,z) 是旋转轴(必须是单位向量)

(xi,yj,zk) 是四元数的虚部,表示旋转方向
注意 :旋转四元数必须是单位四元数,即满足:
∣ q ∣ = w 2 + x 2 + y 2 + z 2 = 1 |q|=\sqrt{w^2+x^2+y^2+z^2}=1 ∣q∣=w2+x2+y2+z2 =1

使用四元数进行 3D 旋转

假设有一个点 v = ( v x , v y , v z ) \mathbf{v}=(v_x,v_y,v_z) v=(vx,vy,vz),我们想用四元数 q 旋转它。方法如下:

  • 将点转换为纯四元数(虚部存储向量坐标)
    p = ( 0 , v x , v y , v z ) p=(0,v_x,v_y,v_z) p=(0,vx,vy,vz)
  • 计算旋转后的点
    p ′ = q p q − 1 p^{\prime}=qpq^{-1} p′=qpq−1
    其中: q − 1 q^{-1} q−1是四元数的逆(单位四元数的逆就是它的共轭)
    旋转后的点 p ′ p^{\prime} p′也是一个纯四元数,其中的虚部给出新坐标。
  • 单位四元数的逆
    q − 1 = q ∗ = ( cos ⁡ θ 2 , − x sin ⁡ θ 2 , − y sin ⁡ θ 2 , − z sin ⁡ θ 2 ) q^{-1}=q^*=(\cos\frac{\theta}{2},-x\sin\frac{\theta}{2},-y\sin\frac{\theta}{2},-z\sin\frac{\theta}{2}) q−1=q∗=(cos2θ,−xsin2θ,−ysin2θ,−zsin2θ)

例程(C语言)

旋转 (1, 0, 0) 向量 绕 Y 轴旋转 90°。

计算后,结果应该接近 (0, 0, -1),即 X 轴向量变成 Z 轴负方向。

c 复制代码
#include <stdio.h>
#include <math.h>

// 定义四元数结构体
typedef struct {
    double w, x, y, z;
} Quaternion;

// 定义向量结构体
typedef struct {
    double x, y, z;
} Vector3;

// 归一化四元数(单位四元数)
Quaternion normalize(Quaternion q) {
    double magnitude = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    q.w /= magnitude;
    q.x /= magnitude;
    q.y /= magnitude;
    q.z /= magnitude;
    return q;
}

// 计算四元数的共轭
Quaternion conjugate(Quaternion q) {
    Quaternion conj = {q.w, -q.x, -q.y, -q.z};
    return conj;
}

// 计算两个四元数的乘法
Quaternion multiply(Quaternion q1, Quaternion q2) {
    Quaternion result;
    result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
    result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
    result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
    result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
    return result;
}

// 旋转向量 v 使用四元数 q
Vector3 rotate_vector(Vector3 v, Quaternion q) {
    Quaternion p = {0, v.x, v.y, v.z}; // 将向量转换为纯四元数
    Quaternion q_conj = conjugate(q);  // 计算四元数共轭

    // 计算旋转后的四元数 p' = q * p * q^(-1)
    Quaternion temp = multiply(q, p);
    Quaternion rotated = multiply(temp, q_conj);

    // 结果的虚部即为旋转后的向量
    Vector3 result = {rotated.x, rotated.y, rotated.z};
    return result;
}

// 生成绕 (ux, uy, uz) 轴旋转 theta 角度的四元数
Quaternion from_axis_angle(double ux, double uy, double uz, double theta) {
    Quaternion q;
    double half_theta = theta * M_PI / 360.0; // 角度转弧度并除以 2
    double sin_half_theta = sin(half_theta);

    q.w = cos(half_theta);
    q.x = ux * sin_half_theta;
    q.y = uy * sin_half_theta;
    q.z = uz * sin_half_theta;

    return normalize(q);
}

int main() {
    // 定义一个向量 (1, 0, 0)
    Vector3 v = {1, 0, 0};

    // 绕 Y 轴旋转 90 度的四元数
    Quaternion q = from_axis_angle(0, 1, 0, 90);

    // 旋转向量
    Vector3 rotated_v = rotate_vector(v, q);

    // 输出旋转后的结果
    printf("旋转后向量: (%f, %f, %f)\n", rotated_v.x, rotated_v.y, rotated_v.z);

    return 0;
}

代码解析

  1. 定义数据结构
    Quaternion 结构体存储四元数(w, x, y, z)
    Vector3 结构体存储 3D 向量(x, y, z)
  2. 归一化四元数
    旋转四元数必须是 单位四元数,所以 normalize() 函数保证四元数的模长为 1。
  3. 计算四元数共轭
    conjugate() 计算 (对于单位四元数,逆就是共轭)。
  4. 四元数乘法
    multiply() 执行两个四元数的乘法,用于计算旋转变换。
  5. 向量旋转
    rotate_vector() 采用公式 计算旋转后的向量。
  6. 从轴-角度转换为四元数
    from_axis_angle() 计算沿任意轴旋转 theta 角度的旋转四元数。

如预期,原来的 (1, 0, 0) 经过 绕 Y 轴旋转 90° 后变成了 (0, 0, -1)

相关推荐
JianZhen✓9 小时前
电商 SKU 选择器:后端只给扁平列表,前端怎么反推规格矩阵并做到 O(1) 命中
前端·矩阵·状态模式
zkk95271 天前
【机器人动力学】(四):动力学回归与最小参数集
机器人·动力学辨识·最小参数集
wechatbot8881 天前
企业微信外部群聊自动回复机器人开发实战
微信·机器人·企业微信·企业微信scrm·企业微信二次开发·企业微信api
wangxiaoxuan2172 天前
如何设计机器人 API:类型、单位、坐标系与失败语义
机器人·科沃斯八界
深蓝学院2 天前
机器人控制算法 MPC 的预测模型从哪来?完整公式推导
机器人·强化学习·机器人控制
蓝天居士2 天前
ROS2从入门到精通(13)
机器人·ros·ros 2
蓝天居士2 天前
JY61P认识、学习与实战(3)
机器人·嵌入式·传感器
feasibility.2 天前
399美元桌面机器人Doly:可爱电子宠物+树莓派开源平台,但不能像ChatGPT那样聊天
机器人·开源·嵌入式·树莓派·diy·桌宠
民乐团扒谱机3 天前
Rakazo 智能清洁机器人深度评测报告
机器人