温故知新，数学之美，欧拉角转四元数

TaylorShi / 2024-11-11 / 原文

简介

要将Roll,Pitch和Yaw转换为四元数，可以按照以下步骤来实现。

这个过程主要是基于欧拉角的旋转顺序（通常是ZYX顺序：Yaw-Pitch-Roll）。四元数是用来表示三维空间中的旋转的数学工具，它避免了欧拉角带来的万向节锁问题。

代码

using System;

public class Quaternion
{
    public double W { get; set; }
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }

    public Quaternion(double w, double x, double y, double z)
    {
        W = w;
        X = x;
        Y = y;
        Z = z;
    }

    public override string ToString()
    {
        return $"Quaternion(W: {W}, X: {X}, Y: {Y}, Z: {Z})";
    }
}

public class Program
{
    public static Quaternion YDR2Quanterion(double roll, double pitch, double yaw)
    {
        // 将角度转换为弧度
        double rollRad = roll * Math.PI / 180.0;
        double pitchRad = pitch * Math.PI / 180.0;
        double yawRad = yaw * Math.PI / 180.0;

        // 计算每个轴的半角三角函数
        double cy = Math.Cos(yawRad * 0.5);
        double sy = Math.Sin(yawRad * 0.5);
        double cp = Math.Cos(pitchRad * 0.5);
        double sp = Math.Sin(pitchRad * 0.5);
        double cr = Math.Cos(rollRad * 0.5);
        double sr = Math.Sin(rollRad * 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);
    }

    public static void Main(string[] args)
    {
        double roll = 30.0;  // 你可以修改这里的值来测试
        double pitch = 45.0;
        double yaw = 60.0;

        Quaternion result = YDR2Quanterion(roll, pitch, yaw);
        Console.WriteLine(result);
    }
}