如何用Python实时处理9轴传感器数据获取头部运动方向？

Got it, let's break down how to build a real-time head movement detection system using Python with your 9-axis sensor data (Gyroscope, Acceleration, Magnetometer). I've tinkered with similar wearable sensor projects before, so here's a practical, step-by-step approach that balances accuracy and real-time performance:

1. 核心逻辑铺垫 #

First off, you can't rely on a single sensor alone—each has its quirks:

• Gyroscope: Tracks rotational speed (deg/s) perfectly for short-term motion, but drifts over time (error accumulates quickly)
• Accelerometer: Measures static tilt relative to gravity, but gets confused by sudden motion acceleration
• Magnetometer: Acts like a compass to track absolute heading, but can be thrown off by nearby metal objects

We'll fuse all three using a lightweight Mahony filter (simpler than Kalman filters for real-time use) to get stable, reliable head orientation angles: Roll (left/right tilt), Pitch (up/down tilt), Yaw (left/right turn).

2. 第一步：数据预处理 #

Before any math, we need to clean up raw sensor data:

• Calibrate sensors: For the gyro, let it sit still for 5-10 seconds, record average values, and subtract this offset from all future readings to eliminate drift. For accelerometer/magnetometer, a quick figure-8 calibration fixes hard/soft iron errors (even basic zero-offset correction helps).
• Real-time data reading: Assuming your device sends data over serial/Bluetooth, use pyserial to stream data. Example snippet:

import serial
import time

ser = serial.Serial('COM3', 9600)  # Replace with your port/baud rate
time.sleep(2)  # Wait for connection to stabilize

def read_sensor_data():
    line = ser.readline().decode('utf-8').strip()
    # Split line into 9 values: gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z, mag_x, mag_y, mag_z
    data = list(map(float, line.split(',')))
    return data[:3], data[3:6], data[6:]  # Return gyro, accel, mag separately

3. 第二步：姿态解算（Mahony滤波） #

This filter combines gyro data for short-term accuracy and accel/mag for long-term drift correction. Here's a Python implementation:

import numpy as np

# Mahony filter parameters (tune based on your sensor's performance)
Kp = 1.0  # Proportional gain
Ki = 0.0  # Integral gain (start with 0, adjust if drift persists)
integral_error = np.zeros(3)
q = np.array([1.0, 0.0, 0.0, 0.0])  # Initial quaternion (no rotation)

def mahony_update(gyro, accel, mag, dt):
    global q, integral_error

    # Convert gyro from deg/s to rad/s
    gyro_rad = np.radians(gyro)

    # Normalize accelerometer and magnetometer data
    accel = accel / np.linalg.norm(accel)
    mag = mag / np.linalg.norm(mag)

    # Compute expected gravity and magnetic field from current quaternion
    q0, q1, q2, q3 = q
    gravity = np.array([
        2*(q1*q3 - q0*q2),
        2*(q0*q1 + q2*q3),
        q0**2 - q1**2 - q2**2 + q3**2
    ])
    mag_north = np.array([
        2*(q0*q3 + q1*q2),
        2*(q2*q3 - q0*q1),
        q0**2 + q1**2 - q2**2 - q3**2
    ])
    mag_east = np.cross(gravity, mag_north)
    mag_east = mag_east / np.linalg.norm(mag_east)
    mag_north = np.cross(mag_east, gravity)

    # Calculate error between measured and expected vectors
    accel_error = np.cross(accel, gravity)
    mag_error = np.cross(mag, mag_north)
    error = accel_error + mag_error

    # Update integral error for drift correction
    integral_error += error * dt

    # Correct gyro data with error gains
    corrected_gyro = gyro_rad + Kp*error + Ki*integral_error

    # Update quaternion to reflect new orientation
    q_dot = 0.5 * np.array([
        -q1*corrected_gyro[0] - q2*corrected_gyro[1] - q3*corrected_gyro[2],
        q0*corrected_gyro[0] + q2*corrected_gyro[2] - q3*corrected_gyro[1],
        q0*corrected_gyro[1] - q1*corrected_gyro[2] + q3*corrected_gyro[0],
        q0*corrected_gyro[2] + q1*corrected_gyro[1] - q2*corrected_gyro[0]
    ])
    q += q_dot * dt
    q = q / np.linalg.norm(q)  # Keep quaternion normalized

    # Convert quaternion to Euler angles (Roll, Pitch, Yaw) in degrees
    roll = np.degrees(np.arctan2(2*(q0*q1 + q2*q3), 1 - 2*(q1**2 + q2**2)))
    pitch = np.degrees(np.arcsin(2*(q0*q2 - q3*q1)))
    yaw = np.degrees(np.arctan2(2*(q0*q3 + q1*q2), 1 - 2*(q2**2 + q3**2)))
    return roll, pitch, yaw

4. 第三步：判断头部运动方向 #

Compare current Euler angles to the previous frame, using thresholds to ignore sensor noise:

# Thresholds (adjust based on your sensor's noise level)
ANGLE_THRESHOLD = 2.0  # Minimum degrees of change to count as movement
prev_roll, prev_pitch, prev_yaw = 0.0, 0.0, 0.0

def detect_movement(current_roll, current_pitch, current_yaw):
    global prev_roll, prev_pitch, prev_yaw
    movement = []

    # Left/right turn (Yaw axis)
    yaw_diff = current_yaw - prev_yaw
    if abs(yaw_diff) > ANGLE_THRESHOLD:
        movement.append("Left Turn" if yaw_diff < 0 else "Right Turn")

    # Up/down tilt (Pitch axis)
    pitch_diff = current_pitch - prev_pitch
    if abs(pitch_diff) > ANGLE_THRESHOLD:
        movement.append("Head Up" if pitch_diff > 0 else "Head Down")

    # Left/right tilt (Roll axis)
    roll_diff = current_roll - prev_roll
    if abs(roll_diff) > ANGLE_THRESHOLD:
        movement.append("Tilt Left" if roll_diff < 0 else "Tilt Right")

    # Update previous angles for next iteration
    prev_roll, prev_pitch, prev_yaw = current_roll, current_pitch, current_yaw
    return movement if movement else ["No Significant Movement"]

5. 实时运行主循环 #

Put it all together in a continuous loop:

if __name__ == "__main__":
    # Calibrate gyro first
    print("Calibrating gyro... Keep head still for 5 seconds.")
    gyro_calib = np.zeros(3)
    sample_count = 100
    for _ in range(sample_count):
        gyro, _, _ = read_sensor_data()
        gyro_calib += np.array(gyro)
        time.sleep(0.05)
    gyro_offset = gyro_calib / sample_count
    print(f"Gyro offset: {gyro_offset}")

    # Start real-time detection
    print("Starting detection...")
    prev_time = time.time()
    try:
        while True:
            current_time = time.time()
            dt = current_time - prev_time
            prev_time = current_time

            # Read and calibrate gyro data
            gyro_raw, accel, mag = read_sensor_data()
            gyro = np.array(gyro_raw) - gyro_offset

            # Update head orientation
            roll, pitch, yaw = mahony_update(gyro, accel, mag, dt)

            # Detect and print movement
            movement = detect_movement(roll, pitch, yaw)
            print(f"Orientation: Roll={roll:.1f}°, Pitch={pitch:.1f}°, Yaw={yaw:.1f}° | Movement: {', '.join(movement)}")

            time.sleep(0.01)  # Adjust loop speed to match sensor update rate
    except KeyboardInterrupt:
        print("Stopping detection.")
        ser.close()

Key Tuning Tips #

• Calibration: Don't skip gyro calibration—it's the biggest factor in reducing drift. Keep magnetometers away from metal during use.
• Threshold Adjustment: If you get false positives, increase ANGLE_THRESHOLD. If you miss small movements, decrease it.
• Real-Time Speed: If the loop lags, reduce print statements or use separate threads for data reading and processing.

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