用Python和Pygame打造MPU6050的3D姿态可视化上位机当你在STM32上成功驱动MPU6050传感器后面对串口终端不断刷新的数字是否曾想过让这些数据活起来本文将带你用Python和Pygame构建一个实时3D可视化界面让传感器姿态变化一目了然。1. 环境准备与基础架构1.1 硬件连接与数据流MPU6050通过I2C与STM32通信STM32再通过串口将姿态数据发送到PC。典型的数据流如下MPU6050 → I2C → STM32 → UART → Python上位机确保你的STM32程序能稳定输出欧拉角或四元数数据。常见的数据格式示例# 欧拉角格式示例 ROLL:23.45,PITCH:-12.34,YAW:56.78\n # 四元数格式示例 QUAT:0.707,0.0,0.0,0.707\n1.2 Python环境配置推荐使用Python 3.7环境需要安装以下库pip install pygame pyserial numpy transforms3d关键库的作用Pygame3D渲染和界面交互Pyserial串口通信Numpy矩阵运算Transforms3d四元数转换提示如果使用虚拟环境建议在项目目录下创建venvpython -m venv mpu6050_venv source mpu6050_venv/bin/activate # Linux/Mac mpu6050_venv\Scripts\activate # Windows2. 串口通信模块实现2.1 串口配置与数据接收创建一个SerialReader类处理串口通信import serial from serial.tools import list_ports class SerialReader: def __init__(self, portNone, baudrate115200): self.ser None self.port port self.baudrate baudrate self.running False def auto_detect_port(self): 自动检测可能的STM32串口 ports list_ports.comports() for port in ports: if STM32 in port.description or USB Serial in port.description: return port.device return None def connect(self): if not self.port: self.port self.auto_detect_port() if not self.port: raise Exception(未检测到可用串口) self.ser serial.Serial( portself.port, baudrateself.baudrate, timeout1 ) self.running True def read_data(self): 读取并解析串口数据 if not self.ser or not self.running: return None line self.ser.readline().decode(ascii, errorsignore).strip() return self._parse_data(line) def _parse_data(self, raw): 解析原始串口数据 # 示例解析欧拉角数据 if raw.startswith(ROLL:): parts raw.split(,) try: roll float(parts[0].split(:)[1]) pitch float(parts[1].split(:)[1]) yaw float(parts[2].split(:)[1]) return {type: euler, roll: roll, pitch: pitch, yaw: yaw} except (IndexError, ValueError): return None # 添加其他数据格式的解析... return None def close(self): if self.ser and self.ser.is_open: self.running False self.ser.close()2.2 数据校验与错误处理在实际应用中我们需要考虑数据完整性和错误处理def _parse_data(self, raw): 增强版数据解析 if not raw or len(raw) 100: # 防止异常长数据 return None # 校验数据格式 if not (raw.startswith((ROLL:, QUAT:)) and , in raw): return None try: if raw.startswith(ROLL:): # 欧拉角解析... elif raw.startswith(QUAT:): parts raw.split(:)[1].split(,) if len(parts) 4: quat [float(x) for x in parts] if sum(x**2 for x in quat) 1.1: # 四元数归一化检查 return None return {type: quat, quat: quat} except (ValueError, IndexError) as e: print(f数据解析错误: {e}) return None return None3. 3D可视化核心实现3.1 Pygame 3D基础使用Pygame进行3D渲染需要一些基础数学知识。我们将实现一个简单的3D立方体它能根据传感器数据旋转。首先定义3D点投影方法import math import pygame import numpy as np def project_3d_to_2d(point_3d, screen_width, screen_height, fov256): 将3D点投影到2D屏幕 x, y, z point_3d # 透视投影 factor fov / (fov z) x_proj x * factor screen_width / 2 y_proj -y * factor screen_height / 2 # Y轴向下 return (int(x_proj), int(y_proj))3.2 立方体模型定义定义一个立方体类包含8个顶点和12条边class Cube: def __init__(self, size100): self.size size half size / 2 # 立方体的8个顶点 (x,y,z) self.vertices [ (-half, -half, -half), (half, -half, -half), (half, half, -half), (-half, half, -half), (-half, -half, half), (half, -half, half), (half, half, half), (-half, half, half) ] # 立方体的12条边 (顶点索引对) self.edges [ (0,1), (1,2), (2,3), (3,0), # 底面 (4,5), (5,6), (6,7), (7,4), # 顶面 (0,4), (1,5), (2,6), (3,7) # 侧面连接线 ] self.faces [ (0,1,2,3), # 底面 (4,5,6,7), # 顶面 (0,1,5,4), # 前面 (2,3,7,6), # 后面 (1,2,6,5), # 右面 (0,3,7,4) # 左面 ] self.colors [ (255,0,0), # 红 (0,255,0), # 绿 (0,0,255), # 蓝 (255,255,0), # 黄 (255,0,255), # 紫 (0,255,255) # 青 ] def rotate(self, quaternion): 用四元数旋转立方体 # 使用transforms3d库进行四元数旋转 from transforms3d.quaternions import quat2mat rot_matrix quat2mat(quaternion) rotated_verts [] for v in self.vertices: # 将顶点转换为numpy数组并旋转 v_arr np.array(v) rotated np.dot(rot_matrix, v_arr) rotated_verts.append(rotated) return rotated_verts3.3 可视化主循环创建主可视化类整合串口读取和3D渲染class MPU6050Visualizer: def __init__(self, width800, height600): pygame.init() self.screen pygame.display.set_mode((width, height)) pygame.display.set_caption(MPU6050 3D姿态可视化) self.clock pygame.time.Clock() self.font pygame.font.SysFont(Arial, 20) self.cube Cube(size150) self.serial_reader SerialReader() self.quaternion [1, 0, 0, 0] # 初始四元数 (无旋转) try: self.serial_reader.connect() except Exception as e: print(f串口连接失败: {e}) self.running False else: self.running True def run(self): while self.running: for event in pygame.event.get(): if event.type pygame.QUIT: self.running False # 读取并处理串口数据 self._update_data() # 渲染 self._render() pygame.display.flip() self.clock.tick(60) # 60 FPS self.serial_reader.close() pygame.quit() def _update_data(self): 从串口更新姿态数据 data self.serial_reader.read_data() if data: if data[type] quat: self.quaternion data[quat] elif data[type] euler: # 将欧拉角转换为四元数 from transforms3d.euler import euler2quat roll math.radians(data[roll]) pitch math.radians(data[pitch]) yaw math.radians(data[yaw]) self.quaternion euler2quat(roll, pitch, yaw, sxyz) def _render(self): 渲染3D立方体和UI self.screen.fill((0, 0, 0)) # 黑色背景 # 旋转立方体并获取顶点 rotated_verts self.cube.rotate(self.quaternion) # 绘制面 for i, face in enumerate(self.cube.faces): points [rotated_verts[v] for v in face] # 简单的背面剔除 - 检查面法线 normal np.cross( np.array(points[1]) - np.array(points[0]), np.array(points[2]) - np.array(points[1]) ) if normal[2] 0: # 只绘制朝向摄像机的面 pygame.draw.polygon( self.screen, self.cube.colors[i], [project_3d_to_2d(p, *self.screen.get_size()) for p in points] ) # 绘制边 for edge in self.cube.edges: points [rotated_verts[edge[0]], rotated_verts[edge[1]]] pygame.draw.line( self.screen, (255, 255, 255), project_3d_to_2d(points[0], *self.screen.get_size()), project_3d_to_2d(points[1], *self.screen.get_size()), 2 ) # 显示四元数数据 quat_text fQuat: {self.quaternion[0]:.3f}, {self.quaternion[1]:.3f}, {self.quaternion[2]:.3f}, {self.quaternion[3]:.3f} text_surface self.font.render(quat_text, True, (255, 255, 255)) self.screen.blit(text_surface, (10, 10))4. 高级功能扩展4.1 数据平滑滤波传感器数据常有噪声添加指数平滑滤波class DataFilter: def __init__(self, alpha0.2): self.alpha alpha self.filtered_quat np.array([1.0, 0.0, 0.0, 0.0]) def update(self, new_quat): new_quat np.array(new_quat) # 确保四元数在同一半球 if np.dot(self.filtered_quat, new_quat) 0: new_quat -new_quat self.filtered_quat self.alpha * new_quat (1 - self.alpha) * self.filtered_quat # 归一化 self.filtered_quat / np.linalg.norm(self.filtered_quat) return self.filtered_quat.tolist() # 在MPU6050Visualizer中初始化 self.filter DataFilter(alpha0.3) # 在_update_data中应用滤波 if data[type] quat: self.quaternion self.filter.update(data[quat])4.2 多模型支持扩展支持更多3D模型如无人机模型class DroneModel: def __init__(self): # 中心主体 self.body_vertices [(-20,-10,-5), (20,-10,-5), (20,10,-5), (-20,10,-5), (-20,-10,5), (20,-10,5), (20,10,5), (-20,10,5)] # 四个螺旋桨臂 self.arms [ [(-30,-30,0), (-50,-50,0), (-50,-50,3), (-30,-30,3)], # 左前 [(30,-30,0), (50,-50,0), (50,-50,3), (30,-30,3)], # 右前 [(-30,30,0), (-50,50,0), (-50,50,3), (-30,30,3)], # 左后 [(30,30,0), (50,50,0), (50,50,3), (30,30,3)] # 右后 ] self.colors { body: (100, 100, 255), arms: [(255,0,0), (0,255,0), (0,0,255), (255,255,0)] } def rotate(self, quaternion): from transforms3d.quaternions import quat2mat rot_matrix quat2mat(quaternion) rotated_body [np.dot(rot_matrix, np.array(v)) for v in self.body_vertices] rotated_arms [] for arm in self.arms: rotated_arms.append([np.dot(rot_matrix, np.array(v)) for v in arm]) return rotated_body, rotated_arms4.3 性能优化技巧顶点缓存优化# 预计算模型顶点减少运行时计算量 self.cached_vertices self.cube.vertices self.cached_edges self.cube.edges self.cached_faces self.cube.faces渲染优化# 使用双缓冲 self.screen pygame.display.set_mode((width, height), pygame.DOUBLEBUF)选择性渲染# 根据距离决定渲染细节 def should_render_detail(distance): return distance 200 # 只对近距离对象渲染细节5. 完整应用集成5.1 主程序入口if __name__ __main__: import argparse parser argparse.ArgumentParser(descriptionMPU6050 3D姿态可视化) parser.add_argument(--port, help指定串口设备, defaultNone) parser.add_argument(--baud, help串口波特率, typeint, default115200) parser.add_argument(--model, help3D模型类型(cube/drone), defaultcube) args parser.parse_args() try: if args.model drone: visualizer DroneVisualizer(portargs.port, baudrateargs.baud) else: visualizer MPU6050Visualizer(portargs.port, baudrateargs.baud) visualizer.run() except KeyboardInterrupt: print(\n程序退出) except Exception as e: print(f错误: {e})5.2 界面增强添加控制面板和状态显示class ControlPanel: def __init__(self, x, y, width, height): self.rect pygame.Rect(x, y, width, height) self.elements [] def add_slider(self, name, min_val, max_val, initial): slider { type: slider, name: name, min: min_val, max: max_val, value: initial, rect: pygame.Rect(self.rect.x 10, self.rect.y 30 len(self.elements)*40, self.rect.width - 20, 20) } self.elements.append(slider) def draw(self, surface): pygame.draw.rect(surface, (50, 50, 50), self.rect) pygame.draw.rect(surface, (100, 100, 100), self.rect, 2) title pygame.font.SysFont(Arial, 16).render(控制面板, True, (255,255,255)) surface.blit(title, (self.rect.x 10, self.rect.y 5)) for element in self.elements: if element[type] slider: pygame.draw.rect(surface, (100,100,100), element[rect]) fill_width int((element[value] - element[min]) / (element[max] - element[min]) * element[rect].width) fill_rect pygame.Rect(element[rect].x, element[rect].y, fill_width, element[rect].height) pygame.draw.rect(surface, (0,200,0), fill_rect) text pygame.font.SysFont(Arial, 12).render( f{element[name]}: {element[value]:.2f}, True, (255,255,255)) surface.blit(text, (element[rect].x, element[rect].y - 20))5.3 数据记录与回放class DataLogger: def __init__(self, filenamempu6050_log.csv): self.filename filename self.file None self.start_time time.time() def start(self): self.file open(self.filename, w) self.file.write(timestamp,quat_w,quat_x,quat_y,quat_z\n) def log(self, quat): if self.file: elapsed time.time() - self.start_time self.file.write(f{elapsed:.3f},{quat[0]},{quat[1]},{quat[2]},{quat[3]}\n) def close(self): if self.file: self.file.close() def replay(self, callback, speed1.0): 回放记录的数据 with open(self.filename, r) as f: next(f) # 跳过标题行 start_time time.time() for line in f: parts line.strip().split(,) timestamp float(parts[0]) / speed quat [float(x) for x in parts[1:5]] # 等待到正确的时间点 while (time.time() - start_time) timestamp: time.sleep(0.001) callback(quat)