什么是欧拉角和四元数

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

什么是欧拉角和四元数？

这里画了一张图，简明方便理解：
在这里插入图片描述
欧拉角 (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轴）顺时针旋转，例如飞机右转向；负值则逆时针旋转，例如飞机左转向

相关工具代码

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


import cn.hutool.core.util.NumberUtil;
import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.NoArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import java.math.BigDecimal;/*** <p>* 四元数与欧拉角转换工具类* </p>** @author admin*/
@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 pitch = Math.asin(2.0 * (qw * qy - qz * qx));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>* 欧拉角 Yaw (偏航角) 转为 四元数* </p>** @param yaw   Yaw (偏航角) 单位：度* @param pitch Pitch (俯仰角) 单位：度* @param roll  Roll (翻滚角) 单位：度* @return {@link Quaternion} 四元数*/public static Quaternion yawToQuaternion(double yaw, double pitch, double roll) {// 将角度转换为弧度double cy = Math.cos(Math.toRadians(yaw) * 0.5);double sy = Math.sin(Math.toRadians(yaw) * 0.5);double cp = Math.cos(Math.toRadians(pitch) * 0.5);double sp = Math.sin(Math.toRadians(pitch) * 0.5);double cr = Math.cos(Math.toRadians(roll) * 0.5);double sr = Math.sin(Math.toRadians(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@AllArgsConstructorpublic static class Quaternion {private double w, x, y, z;}
}

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

import java.math.BigDecimal;
import java.math.RoundingMode;/*** 角度弧度相互转换工具类** @author admin*/
public class AngleConverterUtil {/*** 将弧度值转换为度数** @param radians 弧度值* @return 对应的度数值, 保留4位小数*/public static double radiansToDegrees(double radians) {double degrees = Math.toDegrees(radians);return Double.parseDouble(String.format("%.4f", degrees));}/*** 将度数值转换为弧度值** @param degrees 度数值* @return 对应的弧度值, 保留4位小数*/public static double degreesToRadians(double degrees) {double radians = Math.toRadians(degrees);return Double.parseDouble(String.format("%.4f", radians));}
}