什么是欧拉角和四元数

涉及机器人调度工作的一些基本概念整理理解

什么是欧拉角和四元数 ？

这里画了一张图，简明方便理解：

欧拉角 (Euler Angles) 是一种描述物体在三维空间旋转姿态的方法，涉及3个旋转角度：偏航（Yaw）、俯仰（Pitch）和滚转（Roll）。

欧拉角和四元数可以相互转换，
四元数 (Quaternion) 是用于描述三维旋转的一种数学工具，具有避免万向节锁（Gimbal Lock）的问题。
四元数由一个标量部分和一个矢量部分组成，通常表示为q=w+xi+yj+zk ，其中x，y，z ，w 是实数

再次大白话理解：
假设空间上的一个点 A点（X1,Y1,Z1） 现在开始要求 它先绕着X轴旋转90度，然后再绕着Y轴旋转30度，最后绕着Z轴旋转80度
那么此时用一组向量来表示A点当前的空间位置（X2,Y2,Z2），这组向量为： （滚转角Roll=90度，俯仰角Pitch=30度，偏航角Yaw=80度）

也就是说：
Roll=90 表示A点已经围绕X轴翻转了四分之一圈
Pitch=30 表示A点已经相对于水平面抬起30度
Yaw=80 表示A点从北向顺时针转至80度处

再来看下正负角度数值表示的含义，在右手坐标系中（如上图所示）：

• 当Pitch 正值时绕水平轴（通常为Y轴）顺时针旋转，例如飞机起飞上升；负值则逆时针旋转，例如飞机俯冲下降

• 当Roll 正值时绕前进方向轴（通常为X轴）顺时针旋转，例如飞机右侧机翼下降，左侧机翼上升；负值则逆时针旋转，例如飞机左侧机翼下降，右侧机翼上升

• 当Yaw 正值时绕垂直轴（通常为Z轴）顺时针旋转，例如飞机右转向；负值则逆时针旋转，例如飞机左转向

相关工具网站

四元数和欧拉角之间转换
https://quaternions.online/

相关工具代码

这里记录一下，方便后面参考：

import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.NoArgsConstructor;
import lombok.extern.slf4j.Slf4j;

/**
 * <p>
 * 四元数与欧拉角转换工具类
 * </p>
 *
 * @author admin
 */
@SuppressWarnings("unused")
@Slf4j
public class EulerAngleQuaternionConverterUtil {

    /**
     * <p>
     * 四元数转为欧拉角
     * </p>
     *
     * @param q 四元数
     * @return {@link double[]} 欧拉角，单位：弧度
     */
    public static double[] quaternionToEulerAngles(Quaternion q) {
        // 规范化四元数，确保安全
        double qw = q.w, qx = q.x, qy = q.y, qz = q.z;
        double norm = Math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
        // 防止除以零
        if (norm > 1e-6) {
            qw /= norm;
            qx /= norm;
            qy /= norm;
            qz /= norm;
        }

        // 计算欧拉角
        double roll = Math.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
        double pitchValue = 2.0 * (qw * qy - qz * qx);
        // 限制在[-1, 1]的范围内
        pitchValue = Math.max(-1.0, Math.min(1.0, pitchValue));
        double pitch = Math.asin(pitchValue);
        double yaw = Math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));

        return new double[]{yaw, pitch, roll};
    }

    /**
     * <p>
     * 欧拉角(只有偏航角) 转为 四元数
     * </p>
     *
     * @param yawDegrees Yaw (偏航角) 单位：度
     * @return {@link Quaternion} 四元数
     */
    public static Quaternion eulerAnglesToQuaternion2(double yawDegrees) {
        return eulerAnglesToQuaternion2(yawDegrees, 0, 0);
    }

    /**
     * <p>
     * 欧拉角 转为 四元数
     * </p>
     *
     * @param yawDegrees   Yaw (偏航角) 单位：度
     * @param pitchDegrees Pitch (俯仰角) 单位：度
     * @param rollDegrees  Roll (翻滚角) 单位：度
     * @return {@link Quaternion} 四元数
     */
    public static Quaternion eulerAnglesToQuaternion2(double yawDegrees, double pitchDegrees, double rollDegrees) {
        // 将角度转换为弧度
        double yaw = Math.toRadians(yawDegrees);
        double pitch = Math.toRadians(pitchDegrees);
        double roll = Math.toRadians(rollDegrees);

        return eulerAnglesToQuaternion(yaw, pitch, roll);
    }

    /**
     * <p>
     * 欧拉角(只有偏航角) 转为 四元数
     * </p>
     *
     * @param yaw Yaw (偏航角) 单位：弧度
     * @return {@link Quaternion} 四元数
     */
    public static Quaternion eulerAnglesToQuaternion(double yaw) {
        return eulerAnglesToQuaternion(yaw, 0, 0);
    }


    /**
     * <p>
     * 欧拉角 转为 四元数
     * </p>
     *
     * @param yaw   Yaw (偏航角) 单位：弧度
     * @param pitch Pitch (俯仰角) 单位：弧度
     * @param roll  Roll (翻滚角) 单位：弧度
     * @return {@link Quaternion} 四元数
     */
    public static Quaternion eulerAnglesToQuaternion(double yaw, double pitch, double roll) {
        // 计算四元数的各个分量，角度已为弧度，直接使用
        double cy = Math.cos(yaw * 0.5);
        double sy = Math.sin(yaw * 0.5);
        double cp = Math.cos(pitch * 0.5);
        double sp = Math.sin(pitch * 0.5);
        double cr = Math.cos(roll * 0.5);
        double sr = Math.sin(roll * 0.5);

        // 计算四元数分量
        double w = cr * cp * cy + sr * sp * sy;
        double x = sr * cp * cy - cr * sp * sy;
        double y = cr * sp * cy + sr * cp * sy;
        double z = cr * cp * sy - sr * sp * cy;

        return new Quaternion(w, x, y, z);
    }

    @Data
    @NoArgsConstructor
    @AllArgsConstructor
    public static class Quaternion {
        private double w, x, y, z;
    }
}

顺便记录下在表示转向时除了角度还可能使用弧度表示转换量，弧度和角度之间的换算如下：

import java.math.BigDecimal;
import java.math.RoundingMode;

/**
 * 角度弧度相互转换工具类
 *
 * @author admin
 */
@SuppressWarnings("unused")
public class AngleConverterUtil {

    /**
     * 将弧度值转换为度数
     *
     * @param radians 弧度值
     * @return 对应的度数值, 保留4位小数
     */
    public static double radiansToDegrees(double radians) {
        double degrees = Math.toDegrees(radians);
        return roundToFourDecimals(degrees);
    }

    /**
     * 将度数值转换为弧度值
     *
     * @param degrees 度数值
     * @return 对应的弧度值, 保留4位小数
     */
    public static double degreesToRadians(double degrees) {
        double radians = Math.toRadians(degrees);
        return roundToFourDecimals(radians);
    }

    /**
     * 保留4位小数的四舍五入方法
     *
     * @param value 原始值
     * @return 保留四位小数的值
     */
    private static double roundToFourDecimals(double value) {
        BigDecimal bd = new BigDecimal(value);
        return bd.setScale(4, RoundingMode.HALF_UP).doubleValue();
    }
}