如何用Python实现CesiumJS的headingPitchRollQuaternion四元数转换？

要实现和Cesium完全一致的四元数转换，核心是处理局部ENU坐标系（东北天）到ECEF坐标系（地球固定坐标系）的旋转映射，再叠加HPR的欧拉角旋转。以下是完整的实现逻辑和代码：

核心逻辑拆解 #

Cesium的headingPitchRollQuaternion本质是两步旋转的组合：

1. 将基于局部ENU坐标系的HPR旋转，转换到ECEF坐标系下
2. 四元数计算严格遵循Cesium定义的ZXY旋转顺序（航向绕UP轴、俯仰绕EAST轴、横滚绕NORTH轴）

完整Python实现 #

依赖numpy处理矩阵和四元数运算：

import numpy as np

# WGS84椭球参数
WGS84_A = 6378137.0
WGS84_F = 1/298.257223563
WGS84_E2 = 2 * WGS84_F - WGS84_F ** 2

def latlon_to_ecef(lat_rad, lon_rad, height=0.0):
    """经纬度（弧度）转ECEF坐标"""
    N = WGS84_A / np.sqrt(1 - WGS84_E2 * np.sin(lat_rad)**2)
    x = (N + height) * np.cos(lat_rad) * np.cos(lon_rad)
    y = (N + height) * np.cos(lat_rad) * np.sin(lon_rad)
    z = (N * (1 - WGS84_E2) + height) * np.sin(lat_rad)
    return np.array([x, y, z])

def enu_to_ecef_matrix(lat_rad, lon_rad):
    """计算ENU到ECEF的转换矩阵（正交矩阵）"""
    sin_lon = np.sin(lon_rad)
    cos_lon = np.cos(lon_rad)
    sin_lat = np.sin(lat_rad)
    cos_lat = np.cos(lat_rad)
    
    return np.array([
        [-sin_lon, -sin_lat * cos_lon, cos_lat * cos_lon],
        [cos_lon,  -sin_lat * sin_lon, cos_lat * sin_lon],
        [0.0,       cos_lat,           sin_lat]
    ])

def matrix_to_quaternion(matrix):
    """3x3旋转矩阵转四元数（w, x, y, z）"""
    m00, m01, m02 = matrix[0]
    m10, m11, m12 = matrix[1]
    m20, m21, m22 = matrix[2]
    
    tr = m00 + m11 + m22
    if tr > 0:
        S = np.sqrt(tr + 1.0) * 2
        w = 0.25 * S
        x = (m21 - m12) / S
        y = (m02 - m20) / S
        z = (m10 - m01) / S
    elif (m00 > m11) and (m00 > m22):
        S = np.sqrt(1.0 + m00 - m11 - m22) * 2
        w = (m21 - m12) / S
        x = 0.25 * S
        y = (m01 + m10) / S
        z = (m02 + m20) / S
    elif m11 > m22:
        S = np.sqrt(1.0 + m11 - m00 - m22) * 2
        w = (m02 - m20) / S
        x = (m01 + m10) / S
        y = 0.25 * S
        z = (m12 + m21) / S
    else:
        S = np.sqrt(1.0 + m22 - m00 - m11) * 2
        w = (m10 - m01) / S
        x = (m02 + m20) / S
        y = (m12 + m21) / S
        z = 0.25 * S
    
    return np.array([w, x, y, z])

def hpr_to_quaternion(heading_rad, pitch_rad, roll_rad):
    """HPR（弧度）转四元数，严格匹配Cesium的ZXY旋转顺序"""
    half_h = heading_rad * 0.5
    sin_h = np.sin(half_h)
    cos_h = np.cos(half_h)
    
    half_p = pitch_rad * 0.5
    sin_p = np.sin(half_p)
    cos_p = np.cos(half_p)
    
    half_r = roll_rad * 0.5
    sin_r = np.sin(half_r)
    cos_r = np.cos(half_r)
    
    # 按照Cesium的ZXY顺序计算四元数分量
    x = sin_h * sin_p * cos_r + cos_h * cos_p * sin_r
    y = sin_h * cos_p * cos_r + cos_h * sin_p * sin_r
    z = cos_h * sin_p * cos_r - sin_h * cos_p * sin_r
    w = cos_h * cos_p * cos_r - sin_h * sin_p * sin_r
    
    return np.array([w, x, y, z])

def quaternion_multiply(a, b):
    """四元数乘法：result = a * b，先应用b的旋转，再应用a的旋转"""
    w1, x1, y1, z1 = a
    w2, x2, y2, z2 = b
    
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    
    return np.array([w, x, y, z])

def heading_pitch_roll_quaternion(center_lat_rad, center_lon_rad, center_height=0.0, heading_rad=0.0, pitch_rad=0.0, roll_rad=0.0):
    """
    复刻Cesium的Transforms.headingPitchRollQuaternion
    返回ECEF坐标系下的四元数（w, x, y, z）
    """
    # 计算ENU到ECEF的旋转矩阵
    enu_to_ecef = enu_to_ecef_matrix(center_lat_rad, center_lon_rad)
    # ECEF到ENU的矩阵是正交矩阵的转置
    ecef_to_enu = enu_to_ecef.T
    # 转成四元数
    ecef_to_enu_quat = matrix_to_quaternion(ecef_to_enu)
    
    # HPR转四元数
    hpr_quat = hpr_to_quaternion(heading_rad, pitch_rad, roll_rad)
    
    # 组合旋转：先应用HPR（ENU坐标系），再转换到ECEF坐标系
    result_quat = quaternion_multiply(ecef_to_enu_quat, hpr_quat)
    # 归一化避免浮点误差
    result_quat = result_quat / np.linalg.norm(result_quat)
    
    return result_quat

使用示例 #

# 中心位置：北京（纬度39.9042°，经度116.4074°），高度0
lat = np.deg2rad(39.9042)
lon = np.deg2rad(116.4074)
height = 0.0

# HPR参数：航向90°（向东），俯仰0°，横滚0°
heading = np.deg2rad(90)
pitch = np.deg2rad(0)
roll = np.deg2rad(0)

# 计算四元数
quat = heading_pitch_roll_quaternion(lat, lon, height, heading, pitch, roll)
print(f"四元数（w, x, y, z）：{quat}")

关键说明 #

1. 角度单位：所有输入角度必须是弧度，使用np.deg2rad()转换角度值
2. 坐标系映射：ENU坐标系的方向完全由中心位置的经纬度决定，这就是为什么必须考虑经纬度的原因
3. 四元数顺序：返回的四元数是(w, x, y, z)格式，和Cesium的Quaternion结构完全一致
4. 归一化：最后对四元数做归一化处理，避免浮点运算累积误差

内容的提问来源于stack exchange，提问作者BOAN