Telerobotics

Today we're going to talk about something crucial for anyone working with Inertial Measurement Units (IMUs) and 3D orientation: quaternions. You've probably encountered Euler angles (roll, pitch, yaw) at some point, and they seem intuitive, right? Well, for IMU work, they are often a headache waiting to happen. Today, I'm going to convince you why quaternions are the superior way to represent and manipulate 3D rotations from your IMU data.

The Allure (and the Lie) of Euler Angles:

Euler angles are tempting because they break down any 3D rotation into three sequential rotations around defined axes (like X, Y, and Z). We can easily visualize tilting forward (pitch), turning sideways (yaw), and leaning (roll). This direct mapping to our intuitive understanding of movement makes them seem straightforward.

However, this seemingly simple representation hides a nasty secret: gimbal lock.

Gimbal Lock: The Euler Angle Villain:

Imagine a physical gimbal system – rings rotating within rings. Gimbal lock occurs when two of these axes align. When this happens, you lose a degree of freedom in your rotation. Suddenly, two seemingly independent rotations become coupled, and you can no longer express a full range of 3D orientations.

Mathematically, gimbal lock manifests as a singularity in the transformation equations between Euler angles and rotation matrices (or other orientation representations). This means that for certain specific orientations (typically when pitch approaches +/- 90 degrees), you lose the ability to independently control all three axes.

Why is Gimbal Lock a Disaster for IMUs?

Data Interpretation Breakdown: When your IMU passes through or near a gimbal lock orientation, the Euler angle outputs become erratic and can jump wildly, even for smooth physical movements. This makes accurate interpretation of your sensor data nearly impossible.
Control System Instability: If you're using IMU-derived Euler angles to control a robotic system or stabilize a platform, gimbal lock can lead to unpredictable and unstable behavior. Imagine a drone suddenly losing its ability to control yaw because it pitched too far!
Interpolation and Filtering Issues: Many sensor fusion algorithms and filtering techniques rely on smooth and continuous representations of orientation. Gimbal lock introduces discontinuities and non-linearities that can severely degrade the performance of these algorithms.

Enter the Hero: Quaternions

Quaternions, a mathematical concept extending complex numbers, offer a robust and elegant solution to the problems posed by Euler angles. A quaternion represents a 3D rotation using four numbers: one real part (often denoted as 'w' or 'q0') and three imaginary parts (often denoted as 'x', 'y', 'z' or 'q1', 'q2', 'q3').

The Advantages of Quaternions for IMU Work:

Gimbal Lock Freedom: The most significant advantage is that quaternions do not suffer from gimbal lock. Their mathematical formulation avoids the singularities inherent in Euler angle representations. This ensures a smooth and continuous representation of orientation throughout the entire range of 3D motion.
Smooth Interpolation: Interpolating between two quaternion orientations (finding intermediate rotations) is well-defined and produces smooth, physically meaningful results using techniques like spherical linear interpolation (Slerp). Interpolating Euler angles can lead to bizarre and non-intuitive paths due to the singularities.
Computational Efficiency: Manipulating quaternions for rotations (e.g., composing rotations, rotating vectors) is often more computationally efficient than using rotation matrices derived from Euler angles. This is crucial for real-time IMU processing on embedded systems.
Compact Representation: While they use four numbers compared to three for Euler angles, quaternions are still a relatively compact way to represent 3D rotations, especially when compared to 3x3 rotation matrices (which have nine elements).
Uniqueness (Mostly): For a given 3D rotation, there are two quaternion representations (q and -q) that represent the same orientation. This "double cover" is a minor inconvenience compared to the severe issues of gimbal lock in Euler angles.

Working with Quaternions in IMU Applications:

IMU Firmware Output: Many modern IMUs and their associated libraries directly provide orientation data as quaternions. This saves you the trouble of converting from raw sensor data (accelerometer, gyroscope, magnetometer) using sensor fusion algorithms.
Sensor Fusion Algorithms: Algorithms like Kalman filters and Madgwick filters often internally represent and update orientation using quaternions due to their stability and smooth behavior.
Rotation Operations: To rotate a vector using a quaternion, you perform a specific quaternion multiplication involving the vector (represented as a pure quaternion) and the rotation quaternion.
Conversion (When Necessary): While it's generally best to stick with quaternions for internal calculations, you might occasionally need to convert them to Euler angles for visualization or human understanding. However, be aware that this conversion can reintroduce the possibility of gimbal lock issues if not handled carefully, and the conversion from a quaternion to Euler angles is not unique.

In Conclusion:

For robust, stable, and accurate 3D orientation tracking and manipulation with IMUs, quaternions are the clear winner over Euler angles. While Euler angles might seem more intuitive initially, their susceptibility to gimbal lock makes them unreliable for many IMU applications. Embrace the power and elegance of quaternions, and you'll save yourself countless headaches and build more reliable and accurate systems.

So, the next time you're working with IMU data, remember this lesson: ditch the angles, and let the quaternions guide your rotations. You'll thank me later.

import numpy as np
import math

def quaternion_to_euler(q):
    """
    Converts a quaternion (w, x, y, z) to Euler angles (roll, pitch, yaw) in radians.
    Handles potential gimbal lock by checking the pitch range.
    """
    w, x, y, z = q

    # Roll (x-axis rotation)
    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    # Pitch (y-axis rotation)
    sinp = 2 * (w * y - z * x)
    if abs(sinp) >= 1:
        pitch = np.sign(sinp) * math.pi / 2 # Use +/- 90 degrees if out of range
    else:
        pitch = np.arcsin(sinp)

    # Yaw (z-axis rotation)
    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

def euler_to_quaternion(roll, pitch, yaw):
    """
    Converts Euler angles (roll, pitch, yaw) in radians to a quaternion (w, x, y, z).
    """
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

    return w, x, y, z

def quaternion_multiply(q1, q2):
    """
    Multiplies two quaternions.
    """
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2

    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

    return w, x, y, z

def rotate_vector_by_quaternion(vector, quaternion):
    """
    Rotates a 3D vector by a given quaternion.
    """
    w, x, y, z = quaternion
    v_x, v_y, v_z = vector

    # Convert vector to a pure quaternion
    vector_q = (0, v_x, v_y, v_z)

    # Perform quaternion multiplication: q * v * q_conjugate
    q_conjugate = (w, -x, -y, -z)
    rotated_q = quaternion_multiply(quaternion_multiply(quaternion, vector_q), q_conjugate)

    return rotated_q[1], rotated_q[2], rotated_q[3]

# --- Working Example ---

print("--- Euler Angles and Potential Gimbal Lock ---")
roll_deg = 0
pitch_deg = 89  # Close to gimbal lock
yaw_deg = 0

print(f"Euler Angles (deg): Roll={roll_deg}, Pitch={pitch_deg}, Yaw={yaw_deg}")
roll_rad = np.deg2rad(roll_deg)
pitch_rad = np.deg2rad(pitch_deg)
yaw_rad = np.deg2rad(yaw_deg)

q_from_euler = euler_to_quaternion(roll_rad, pitch_rad, yaw_rad)
print(f"Quaternion from Euler: {q_from_euler:.4f}")

# Introduce a small change in yaw
yaw_deg_perturbed = 10
yaw_rad_perturbed = np.deg2rad(yaw_deg_perturbed)
q_from_euler_perturbed = euler_to_quaternion(roll_rad, pitch_rad, yaw_rad_perturbed)
print(f"Quaternion from Perturbed Yaw: {q_from_euler_perturbed:.4f}")

# Convert back to Euler angles
roll_back, pitch_back, yaw_back = quaternion_to_euler(q_from_euler)
print(f"Euler Angles back (deg): Roll={np.rad2deg(roll_back):.2f}, Pitch={np.rad2deg(pitch_back):.2f}, Yaw={np.rad2deg(yaw_back):.2f}")

roll_back_perturbed, pitch_back_perturbed, yaw_back_perturbed = quaternion_to_euler(q_from_euler_perturbed)
print(f"Euler Angles back (deg) - Perturbed Yaw: Roll={np.rad2deg(roll_back_perturbed):.2f}, Pitch={np.rad2deg(pitch_back_perturbed):.2f}, Yaw={np.rad2deg(yaw_back_perturbed):.2f}")
print("Notice how a small change in yaw near high pitch can cause large changes in roll/yaw when converting back.")

print("\n--- Quaternion Rotation ---")
initial_vector = (1, 0, 0)  # A vector pointing along the x-axis
rotation_degrees = (0, 45, 0) # Rotate 45 degrees around the y-axis
rotation_radians = np.deg2rad(rotation_degrees)
rotation_quaternion = euler_to_quaternion(*rotation_radians)
print(f"Rotation Quaternion (for 45 deg around y): {rotation_quaternion:.4f}")

rotated_vector = rotate_vector_by_quaternion(initial_vector, rotation_quaternion)
print(f"Initial Vector: {initial_vector}")
print(f"Rotated Vector: {rotated_vector:.4f}")

print("\n--- Smooth Quaternion Interpolation (SLERP - simplified) ---")
def slerp_simplified(q1, q2, t):
    """
    Simplified SLERP for demonstration (assumes normalized quaternions).
    """
    dot = np.dot(q1, q2)

    # Ensure shortest path
    if dot < 0:
        q2 = -np.array(q2)
        dot = -dot

    if dot > 0.9995:
        # Quaternions are very close, linear interpolation is sufficient
        result = q1 + t * (q2 - q1)
        return result / np.linalg.norm(result)

    theta_0 = np.arccos(np.clip(dot, -1, 1))
    sin_theta_0 = np.sin(theta_0)
    theta = theta_0 * t
    sin_theta = np.sin(theta)

    s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
    s1 = sin_theta / sin_theta_0

    result = (s0 * np.array(q1)) + (s1 * np.array(q2))
    return result / np.linalg.norm(result)

q_start = (1, 0, 0, 0)  # No rotation
q_end = euler_to_quaternion(np.deg2rad(30), np.deg2rad(60), np.deg2rad(90))

num_steps = 5
for i in range(num_steps + 1):
    t = i / num_steps
    q_interpolated = slerp_simplified(q_start, q_end, t)
    euler_interpolated = quaternion_to_euler(q_interpolated)
    print(f"t={t:.2f}, Quaternion={q_interpolated:.4f}, Euler (deg)={np.rad2deg(euler_interpolated):.2f}")
print("Notice the smooth transition in both quaternion and Euler angles during interpolation.")

Explanation of the Code:

quaternion_to_euler(q):
- Takes a quaternion (w, x, y, z) as input.
- Implements the standard formulas to convert a quaternion to roll, pitch, and yaw (in radians).
- Includes a check for potential gimbal lock around pitch = +/- 90 degrees to avoid NaN values from arcsin.
euler_to_quaternion(roll, pitch, yaw):
- Takes Euler angles (roll, pitch, yaw) in radians as input.
- Implements the standard formulas to convert Euler angles to a quaternion (w, x, y, z).
quaternion_multiply(q1, q2):
- Performs quaternion multiplication of two quaternions. This is essential for composing rotations.
rotate_vector_by_quaternion(vector, quaternion):
- Demonstrates how to rotate a 3D vector using a quaternion.
- Converts the vector into a pure quaternion (with a real part of 0).
- Applies the quaternion rotation formula: q * v * q_conjugate, where v is the pure quaternion representing the vector and q_conjugate is the conjugate of the rotation quaternion.
Working Example:
- Euler Angles and Potential Gimbal Lock:
  - Sets Euler angles with a pitch close to 90 degrees (a region prone to gimbal lock).
  - Converts these Euler angles to a quaternion and then back to Euler angles.
  - Introduces a small perturbation in the yaw angle and repeats the conversion.
  - Observation: Notice how a small change in yaw near a high pitch value can result in significant and potentially unexpected changes in the recovered roll and yaw angles. This illustrates the instability around gimbal lock.
- Quaternion Rotation:
  - Defines an initial vector and a rotation in Euler angles.
  - Converts the Euler rotation to a quaternion.
  - Rotates the vector using the quaternion multiplication method.
- Smooth Quaternion Interpolation (Simplified SLERP):
  - Defines two quaternions representing different orientations.
  - Implements a simplified version of Spherical Linear Interpolation (SLERP) to find intermediate rotations between the two quaternions.
  - Converts the interpolated quaternions back to Euler angles for visualization.
  - Observation: Notice how the quaternion interpolation produces a smooth and gradual change in both the quaternion components and the resulting Euler angles, even through orientations that might cause issues with direct Euler angle interpolation.

How to Use This Code for IMU Work:

IMU Data: If your IMU directly outputs quaternion data, you can use that directly for orientation representation and manipulation.
Sensor Fusion: If you're implementing your own sensor fusion (e.g., using accelerometer and gyroscope data to estimate orientation), you would typically perform the fusion calculations directly on quaternions to avoid gimbal lock. Libraries like scipy.spatial.transform.Rotation in Python provide tools for quaternion manipulation and conversion.
Rotation and Transformation: Use the rotate_vector_by_quaternion function to rotate sensor readings (like acceleration vectors) from the sensor frame to a world frame based on the IMU's orientation.
Interpolation: If you need to animate or smooth orientation data, use quaternion interpolation techniques like Slerp for more stable and visually correct results.
Conversion (Use with Caution): Only convert quaternions to Euler angles for final visualization or when a human-readable format is strictly required, and be mindful of the potential for gimbal lock artifacts in the Euler angle representation.

This working example provides a foundation for understanding and using quaternions in your IMU projects. Remember to prioritize quaternion-based operations for internal calculations and only convert to Euler angles when absolutely necessary for human interpretation.