如何在Python中用Matplotlib绘制含Roll、Pitch、Yaw的6D图像？

Hey there! Let's clear up that "6D visualization" confusion first—what you really need for your quadrotor's Roll/Pitch/Yaw is 3D spatial visualization of rotational姿态 (attitude). Since you already know 3D plotting in Matplotlib, we can build on that to create intuitive, dynamic representations of your drone's orientation.

Here's a step-by-step breakdown with code examples tailored to your use case:

Core Idea #

Instead of trying to plot 6 dimensions (which isn't visually feasible), we'll visualize your quadrotor's attitude by drawing a 3D model (or coordinate axes) that rotates according to your Roll/Pitch/Yaw data. This makes the rotational changes immediately visible.

Step 1: Convert Euler Angles to Rotation Matrices #

Matplotlib works with coordinate transformations, so we first convert your Roll (X-axis), Pitch (Y-axis), Yaw (Z-axis) values into a rotation matrix. This matrix will transform our base model to match the drone's current orientation.

Step 2: Define a Quadrotor Model #

We'll use a simple cross shape for the drone's arms, plus colored coordinate axes to clearly show Roll/Pitch/Yaw directions.

Step 3: Plot Static or Animated Attitude #

You can plot a single static姿态 for analysis, or create an animation to show how the drone's orientation changes over time (perfect for camera-derived sequential data).

Example 1: Static Attitude Plot #

This code draws a single quadrotor姿态 with labeled axes to map Roll/Pitch/Yaw to visual space:

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def euler_to_rotation_matrix(roll, pitch, yaw):
    # Convert Euler angles (Roll-X, Pitch-Y, Yaw-Z, right-hand coordinate system) to rotation matrix
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])
    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    # Combine rotations: R = Yaw * Pitch * Roll
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R

# Sample attitude data (radians)
roll_angle = np.pi/4  # 45° roll
pitch_angle = np.pi/6 # 30° pitch
yaw_angle = np.pi/3   # 60° yaw

# Define base quadrotor model and coordinate axes
center = np.array([0, 0, 0])
# Arm endpoints (length = 1)
arms = np.array([[1,0,0], [-1,0,0], [0,1,0], [0,-1,0]])
# Colored axes (X=red, Y=green, Z=blue)
axes = np.array([[1,0,0], [0,1,0], [0,0,1]])

# Calculate rotated coordinates
rotation_matrix = euler_to_rotation_matrix(roll_angle, pitch_angle, yaw_angle)
rotated_arms = np.dot(arms, rotation_matrix.T)
rotated_axes = np.dot(axes, rotation_matrix.T)

# Plot the visualization
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111, projection='3d')

# Draw rotated coordinate axes
ax.quiver(center[0], center[1], center[2], 
          rotated_axes[0,0], rotated_axes[0,1], rotated_axes[0,2], 
          color='r', label='X (Roll)')
ax.quiver(center[0], center[1], center[2], 
          rotated_axes[1,0], rotated_axes[1,1], rotated_axes[1,2], 
          color='g', label='Y (Pitch)')
ax.quiver(center[0], center[1], center[2], 
          rotated_axes[2,0], rotated_axes[2,1], rotated_axes[2,2], 
          color='b', label='Z (Yaw)')

# Draw quadrotor arms
for arm in rotated_arms:
    ax.plot([center[0], arm[0]], [center[1], arm[1]], [center[2], arm[2]], 
            color='gray', linewidth=3)

# Configure plot settings
ax.set_xlim([-1.5, 1.5])
ax.set_ylim([-1.5, 1.5])
ax.set_zlim([-1.5, 1.5])
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax.set_title('Quadrotor Attitude (Roll=45°, Pitch=30°, Yaw=60°)')
ax.legend()
plt.show()

Example 2: Animated Attitude Sequence #

If you have sequential Roll/Pitch/Yaw data from your camera, this animation will play through the drone's orientation changes in real time:

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.animation import FuncAnimation

def euler_to_rotation_matrix(roll, pitch, yaw):
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])
    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    return np.dot(R_z, np.dot(R_y, R_x))

# Simulate sequential attitude data (replace with your camera data)
time_steps = np.linspace(0, 10, 100)
roll_sequence = np.sin(time_steps) * np.pi/4
pitch_sequence = np.cos(time_steps) * np.pi/6
yaw_sequence = time_steps * np.pi/10

# Base model definition
center = np.array([0, 0, 0])
arms = np.array([[1,0,0], [-1,0,0], [0,1,0], [0,-1,0]])
axes = np.array([[1,0,0], [0,1,0], [0,0,1]])

# Initialize plot
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim([-1.5, 1.5])
ax.set_ylim([-1.5, 1.5])
ax.set_zlim([-1.5, 1.5])
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax.set_title('Quadrotor Attitude Animation')

# Initialize plot objects
arm_lines = [ax.plot([], [], [], color='gray', linewidth=3)[0] for _ in range(4)]
axis_quivers = [
    ax.quiver(center[0], center[1], center[2], 0,0,0, color='r'),
    ax.quiver(center[0], center[1], center[2], 0,0,0, color='g'),
    ax.quiver(center[0], center[1], center[2], 0,0,0, color='b')
]
status_text = ax.text2D(0.05, 0.95, '', transform=ax.transAxes)

def update(frame):
    # Get current attitude values
    r = roll_sequence[frame]
    p = pitch_sequence[frame]
    y = yaw_sequence[frame]
    R = euler_to_rotation_matrix(r, p, y)
    
    # Update quadrotor arms
    rotated_arms = np.dot(arms, R.T)
    for i, arm in enumerate(rotated_arms):
        arm_lines[i].set_data([center[0], arm[0]], [center[1], arm[1]])
        arm_lines[i].set_3d_properties([center[2], arm[2]])
    
    # Update coordinate axes
    rotated_axes = np.dot(axes, R.T)
    axis_quivers[0].set_segments([[[0,0,0], rotated_axes[0]]])
    axis_quivers[1].set_segments([[[0,0,0], rotated_axes[1]]])
    axis_quivers[2].set_segments([[[0,0,0], rotated_axes[2]]])
    
    # Update status text
    status_text.set_text(
        f'Time: {time_steps[frame]:.1f}s\n'
        f'Roll: {np.degrees(r):.1f}°\n'
        f'Pitch: {np.degrees(p):.1f}°\n'
        f'Yaw: {np.degrees(y):.1f}°'
    )
    return arm_lines + axis_quivers + [status_text]

# Create and run animation
ani = FuncAnimation(fig, update, frames=len(time_steps), interval=50, blit=True)
plt.show()

Key Notes #

• Euler Angle Order: The code uses Roll-X → Pitch-Y → Yaw-Z rotation order (right-hand coordinate system). Adjust the rotation matrix multiplication order if your quadrotor uses a different convention.
• Real-Time Data: For camera-derived live data, replace the simulated roll_sequence/pitch_sequence/yaw_sequence with a function that reads your latest sensor values.
• Model Customization: You can replace the simple cross with a more detailed quadrotor model (e.g., adding propellers) by defining additional 3D points and rotating them with the same matrix.

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