基于Wiiuse库从MotionPlus陀螺仪角速度计算姿态四元数/欧拉角的技术方案咨询

Got it, let's walk through how to turn that Wii MotionPlus gyro data into stable, DSU-compatible quaternions for your robot TCP teaching project. You're already halfway there having the raw angle rates—here's the actionable stuff:

First, Why Quaternions Instead of Euler Angles? #

Euler angles suffer from gimbal lock, which is a nightmare for robot姿态 control. Quaternions avoid that, they're compact, and they match the DSU Controller output format you want—so this is definitely the right path.

Core Approach: Sensor Fusion #

Wii Remote has both a 3-axis accelerometer and the MotionPlus gyro. You need to fuse these two data streams:

• Gyro gives high-rate angular velocity (great for short-term accuracy) but drifts over time.
• Accelerometer measures gravity (great for long-term attitude calibration) but is noisy with movement.

A complementary filter is perfect here—it's simple, fast, and works well for real-time applications like robot control (no need for the complexity of a Kalman filter unless you need ultra-precise data).

Code Implementation (C++ + Wiiuse) #

Here's a drop-in solution that integrates with Wiiuse and outputs DSU-style quaternions:

Step 1: Define Quaternion Structure #

typedef struct {
    double w;  // Scalar component (matches DSU's first value)
    double x;  // X-axis vector component
    double y;  // Y-axis vector component
    double z;  // Z-axis vector component
} Quaternion;

Step 2: Complementary Filter for Quaternion Update #

This function takes raw gyro/accel data, time between samples, and updates the quaternion:

#include <math.h>

void update_quaternion(Quaternion *q, double gyro_x, double gyro_y, double gyro_z, 
                       double acc_x, double acc_y, double acc_z, double dt) {
    // Convert gyro from degrees/sec to radians/sec (Wiiuse outputs degrees)
    gyro_x *= M_PI / 180.0;
    gyro_y *= M_PI / 180.0;
    gyro_z *= M_PI / 180.0;

    // 1. Gyro integration to update quaternion
    const double half_dt = dt * 0.5;
    const double wx = gyro_x * half_dt;
    const double wy = gyro_y * half_dt;
    const double wz = gyro_z * half_dt;

    const double qw = q->w;
    const double qx = q->x;
    const double qy = q->y;
    const double qz = q->z;

    // Apply quaternion rotation differential equation
    q->w = qw - wx*qx - wy*qy - wz*qz;
    q->x = qx + wx*qw + wy*qz - wz*qy;
    q->y = qy + wy*qw + wz*qx - wx*qz;
    q->z = qz + wz*qw + wx*qy - wy*qx;

    // Normalize quaternion to prevent drift from numerical errors
    double norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;

    // 2. Accelerometer correction to fix gyro drift
    if (acc_x != 0 || acc_y != 0 || acc_z != 0) {
        // Calculate gravity vector from current quaternion
        const double gravity_x = 2*(qx*qz - qw*qy);
        const double gravity_y = 2*(qw*qx + qy*qz);
        const double gravity_z = qw*qw - qx*qx - qy*qy + qz*qz;

        // Normalize accelerometer data (convert to unit vector)
        double acc_norm = sqrt(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z);
        acc_x /= acc_norm;
        acc_y /= acc_norm;
        acc_z /= acc_norm;

        // Calculate error between measured gravity and expected gravity
        const double error_x = acc_y*gravity_z - acc_z*gravity_y;
        const double error_y = acc_z*gravity_x - acc_x*gravity_z;
        const double error_z = acc_x*gravity_y - acc_y*gravity_x;

        // Tunable correction factor (start with 0.01, adjust based on noise/drift)
        const double kp = 0.01;

        // Correct gyro values with error
        gyro_x += kp * error_x;
        gyro_y += kp * error_y;
        gyro_z += kp * error_z;

        // Re-update quaternion with corrected gyro data
        const double wx_corrected = gyro_x * half_dt;
        const double wy_corrected = gyro_y * half_dt;
        const double wz_corrected = gyro_z * half_dt;

        q->w = qw - wx_corrected*qx - wy_corrected*qy - wz_corrected*qz;
        q->x = qx + wx_corrected*qw + wy_corrected*qz - wz_corrected*qy;
        q->y = qy + wy_corrected*qw + wz_corrected*qx - wx_corrected*qz;
        q->z = qz + wz_corrected*qw + wx_corrected*qy - wy_corrected*qx;

        // Re-normalize
        norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
        q->w /= norm;
        q->x /= norm;
        q->y /= norm;
        q->z /= norm;
    }
}

Step 3: Integrate with Wiiuse Callback #

Hook this into your Wiiuse event loop to process real-time data:

#include <wiiuse.h>
#include <time.h>

Quaternion current_pose = {1.0, 0.0, 0.0, 0.0};  // Initial upright pose
double last_sample_time = 0.0;

void wiimote_callback(struct wiimote_t* wm, int mesg_count, struct mesg* mesg) {
    for (int i = 0; i < mesg_count; ++i) {
        if (mesg[i].type == WIIUSE_MOTIONPLUS) {
            // Get raw gyro data from MotionPlus
            const double gyro_x = mesg[i].motionplus.angle_rate[0];
            const double gyro_y = mesg[i].motionplus.angle_rate[1];
            const double gyro_z = mesg[i].motionplus.angle_rate[2];

            // Get accelerometer data from Wii Remote
            const double acc_x = wm->accel.x;
            const double acc_y = wm->accel.y;
            const double acc_z = wm->accel.z;

            // Calculate time since last sample (in seconds)
            const double current_time = (double)clock() / CLOCKS_PER_SEC;
            const double dt = current_time - last_sample_time;
            last_sample_time = current_time;

            // Update our pose quaternion
            update_quaternion(&current_pose, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, dt);

            // Output in DSU Controller format (w, x, y, z)
            printf("DSU Quaternion: w=%.4f, x=%.4f, y=%.4f, z=%.4f\n", 
                   current_pose.w, current_pose.x, current_pose.y, current_pose.z);
        }
    }
}

Critical Tuning Tips #

1. Accelerometer Calibration: Before using, set the Wii Remote on a flat surface and record the raw accel values. Subtract these offsets from the incoming accel data to zero out gravity when stationary—this fixes drift.
2. Adjust kp: If your quaternion jitters too much, lower kp (try 0.005). If drift is bad, raise it (max ~0.02).
3. Sample Rate: Wii MotionPlus runs at ~100Hz, so aim to process data at this rate to keep dt consistent (avoid large jumps in time between samples).
4. Magnetometer (Optional): If you have the Wii Remote's MotionPlus with built-in magnetometer, add it to the fusion to get absolute heading (no yaw drift). The code can be extended with a similar correction step for magnetic north.

Integrating with Your Robot #

Once you have the quaternion, most modern robot controllers accept quaternion inputs directly for TCP姿态 commands. If your robot requires Euler angles (RPY), you can convert the quaternion using this formula:

// Convert quaternion to Roll-Pitch-Yaw (Euler angles, radians)
double roll = atan2(2*(current_pose.w*current_pose.x + current_pose.y*current_pose.z), 
                    1 - 2*(current_pose.x*current_pose.x + current_pose.y*current_pose.y));
double pitch = asin(2*(current_pose.w*current_pose.y - current_pose.z*current_pose.x));
double yaw = atan2(2*(current_pose.w*current_pose.z + current_pose.x*current_pose.y), 
                   1 - 2*(current_pose.y*current_pose.y + current_pose.z*current_pose.z));

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