使用python提供一段欧拉角转换为四元数的公式

欧拉角和四元数都可以用来表示旋转。欧拉角是三个角度，分别表示绕三个轴旋转的角度；而四元数是一个4元组，包含一个实数和三个虚数，表示旋转的角度和旋转轴。下面是欧拉角转换为四元数的公式：

首先，将欧拉角转换为旋转矩阵，然后将旋转矩阵转换为四元数。

欧拉角转换为旋转矩阵的公式：

$R_{xyz}=R_z(\theta_3)R_y(\theta_2)R_x(\theta_1)$

其中，$R_x(\theta_1)$、$R_y(\theta_2)$、$R_z(\theta_3)$ 分别表示绕 X 轴、Y 轴、Z 轴旋转的矩阵，$\theta_1$、$\theta_2$、$\theta_3$ 分别表示绕 X 轴、Y 轴、Z 轴旋转的角度。

将旋转矩阵转换为四元数的公式：

$q_w=\frac{1}{2}\sqrt{1+tr(R_{xyz})}$

$q_x=\frac{R_{32}-R_{23}}{4q_w}$

$q_y=\frac{R_{13}-R_{31}}{4q_w}$

$q_z=\frac{R_{21}-R_{12}}{4q_w}$

其中，$tr(R_{xyz})=R_{11}+R_{22}+R_{33}$，$q_w$、$q_x$、$q_y$、$q_z$ 分别表示四元数的实部和虚部。

下面是 Python 代码实现：

import math

def euler_to_quaternion(phi, theta, psi):
    # 将欧拉角转换为弧度
    phi = math.radians(phi)
    theta = math.radians(theta)
    psi = math.radians(psi)
    # 计算旋转矩阵
    R_x = [[1, 0, 0],
           [0, math.cos(phi), -math.sin(phi)],
           [0, math.sin(phi), math.cos(phi)]]
    R_y = [[math.cos(theta), 0, math.sin(theta)],
           [0, 1, 0],
           [-math.sin(theta), 0, math.cos(theta)]]
    R_z = [[math.cos(psi), -math.sin(psi), 0],
           [math.sin(psi), math.cos(psi), 0],
           [0, 0, 1]]
    R_xyz = [[0, 0, 0],
             [0, 0, 0],
             [0, 0, 0]]
    for i in range(3):
        for j in range(3):
            for k in range(3):
                R_xyz[i][j] += R_z[i][k] * R_y[k][j]
            R_xyz[i][j] = round(R_xyz[i][j], 8)
        for k in range(3):
            R_xyz[i][j] += R_x[i][k] * R_xyz[k][j]
        R_xyz[i][j] = round(R_xyz[i][j], 8)
    # 计算四元数
    qw = 0.5 * math.sqrt(1 + R_xyz[0][0] + R_xyz[1][1] + R_xyz[2][2])
    qx = (R_xyz[2][1] - R_xyz[1][2]) / (4 * qw)
    qy = (R_xyz[0][2] - R_xyz[2][0]) / (4 * qw)
    qz = (R_xyz[1][0] - R_xyz[0][1]) / (4 * qw)
    return qw, qx, qy, qz

其中，phi、theta、psi 分别表示绕 X 轴、Y 轴、Z 轴旋转的角度，返回值 qw、qx、qy、qz 分别表示四元数的实部和虚部。