Python基于pyglet/pywavefront/OpenGL通过四元数设置3D对象朝向

实现目标 #

• 复刻开源JS项目的四元数传感器3D可视化效果，将渲染对象替换为火箭3D模型，开发语言改为Python
• 直接使用串口周期性获取的四元数数据设置模型绝对朝向，替代原有的增量式相对旋转
• 简化初始位姿配置逻辑，去掉试错得到的零散变换代码
• 修复两个现存问题：传感器倒置导致的模型翻转、显示器与磁北不对齐导致的初始偏航角偏差

核心实现逻辑 #

OpenGL固定渲染管线下，绝对姿态设置不能靠逐次叠加glRotated增量旋转，否则会出现累积误差。正确做法是每帧绘制前重置模型视图矩阵，将四元数转换为4x4旋转矩阵直接乘入当前矩阵栈，保证每次旋转都是相对于初始姿态的绝对设置。

问题修复方案 #

• 传感器倒置修正：不需要修改渲染矩阵逻辑，直接在原始传感器四元数上预乘一个固定安装校正四元数即可。倒置安装通常对应绕X轴旋转180度，校正四元数可设为Quaternion(0, 1, 0, 0)，如果实际翻转方向不对可调整校正四元数的分量。
• Z轴初始偏航补偿：不要在矩阵变换中插入欧拉角旋转，避免万向锁问题。首次启动时记录初始偏航角，生成绕Z轴旋转-init_yaw的补偿四元数，和姿态四元数做预乘后再转旋转矩阵即可。
• 初始位姿简化：加载模型后先计算模型包围盒，将模型原点平移到自身几何中心，不需要反复试错凑平移参数；所有初始变换整合为缩放、居中、设置相机距离三步，去掉零散的循环旋转、平移试错代码。

完整实现代码 #

import pyglet
import pywavefront
from pywavefront import visualization
from pyglet.gl import *
from pyglet.window import key
import numpy as np
from squaternion import Quaternion

# 配置参数
MODEL_PATH = '../models/10475_Rocket_Ship_v1_L3.obj'
CAMERA_DISTANCE = 200
MODEL_SCALE = 0.75
YAW_CORRECTION = None
# 传感器倒置校正四元数：绕X轴转180度，可根据实际安装方向调整
MOUNT_CORRECT_Q = Quaternion(0, 1, 0, 0)

window = pyglet.window.Window(resizable=True)
window.projection = pyglet.window.Projection3D(zfar=1000)
scene = pywavefront.Wavefront(MODEL_PATH, collect_faces=True)

# 计算模型包围盒，自动居中
vertices = np.array(scene.vertices)
bbox_min = vertices.min(axis=0)
bbox_max = vertices.max(axis=0)
model_center = (bbox_min + bbox_max) / 2

current_q = Quaternion(1, 0, 0, 0)

@window.event
def on_draw():
    global YAW_CORRECTION
    window.clear()
    
    # 重置矩阵栈
    glPopMatrix()
    glPushMatrix()
    
    # 相机位置偏移
    glTranslated(0, 0, CAMERA_DISTANCE)
    
    # 四元数校正处理
    corrected_q = MOUNT_CORRECT_Q * current_q
    euler = corrected_q.to_euler(degrees=True)
    
    # 首次启动记录初始偏航角
    if YAW_CORRECTION is None:
        YAW_CORRECTION = euler[2]
    # 偏航补偿
    yaw_compensate_q = Quaternion.from_euler(0, 0, -YAW_CORRECTION, degrees=True)
    final_q = yaw_compensate_q * corrected_q
    
    # 四元数转4x4旋转矩阵（注意OpenGL是列主序）
    rot_mat = np.array(final_q.to_rot())
    mat4x4 = np.array([
        [rot_mat[0,0], rot_mat[1,0], rot_mat[2,0], 0],
        [rot_mat[0,1], rot_mat[1,1], rot_mat[2,1], 0],
        [rot_mat[0,2], rot_mat[1,2], rot_mat[2,2], 0],
        [0, 0, 0, 1]
    ], dtype=np.float64)
    
    glMultMatrixd(mat4x4)
    
    # 模型居中+缩放
    glTranslated(0, 0, -CAMERA_DISTANCE)
    glTranslated(-model_center[0], -model_center[1], -model_center[2])
    glScaled(MODEL_SCALE, MODEL_SCALE, MODEL_SCALE)
    
    visualization.draw(scene)

@window.event
def on_key_press(symbol, modifiers):
    global current_q
    # 测试用按键切换姿态
    if symbol == key.Q:
        current_q = Quaternion(1, 0, 0, 0)
    if symbol == key.W:
        current_q = Quaternion(0, 1, 0, 0)

if __name__ == "__main__":
    glEnable(GL_DEPTH_TEST)
    glClearColor(0.85, 0.85, 0.85, 1)
    # 初始化矩阵栈
    glPushMatrix()
    pyglet.app.run()

串口数据接入说明 #

串口读取四元数的逻辑已经调试完成的话，只需要在串口数据回调函数里把读到的w,x,y,z值赋值给全局变量current_q即可，不需要修改渲染部分逻辑，渲染帧会自动读取最新四元数更新姿态。

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