基于四元数的ENU与NED坐标系双向转换技术问询

Hey there! Let's dive into converting between ENU and NED coordinate systems—both for vectors and quaternions—with a focus on the core principles first, since that's what you're most interested in. We'll wrap up with Python code to put it all into practice.

First, let's align on coordinate definitions to avoid confusion:

• ENU: X=East, Y=North, Z=Up (right-handed system)
• NED: X=North, Y=East, Z=Down (right-handed system)

The mapping you identified is exactly right:

• ENU's East (X) maps directly to NED's East (Y)
• ENU's North (Y) maps directly to NED's North (X)
• ENU's Up (Z) maps to NED's Down (-Z, since Down is the opposite direction of Up)

So the conversion rules are dead simple:

• ENU → NED: v_NED = [v_ENU[Y], v_ENU[X], -v_ENU[Z]]
• NED → ENU: v_ENU = [v_NED[Y], v_NED[X], -v_NED[Z]]

Your example checks out perfectly: [5,2,1] (ENU) becomes [2,5,-1] (NED) when applying this rule.

Roll/Pitch/Yaw (RPY) vectors represent orientation as sequential rotations around coordinate axes, but their conversion depends entirely on your specific RPY definition (e.g., fixed vs. body axes, rotation order like XYZ vs. ZYX).

If we assume a standard fixed-axis XYZ order (Roll = rotation around X, Pitch = rotation around Y, Yaw = rotation around Z, right-handed), we can map axes directly using the same ENU↔NED correspondence:

• ENU Roll (X-axis, East) → NED Pitch (Y-axis, East)
• ENU Pitch (Y-axis, North) → NED Roll (X-axis, North)
• ENU Yaw (Z-axis, Up) → NED -Yaw (since NED's Z is Down, opposite of ENU's Up)

So the conversion rules would be:

• RPY_NED = [RPY_ENU[1], RPY_ENU[0], -RPY_ENU[2]]
• RPY_ENU = [RPY_NED[1], RPY_NED[0], -RPY_NED[2]]

⚠️ Note: This only works if your RPY uses the same axis order and handedness across both coordinate systems. For more reliable results, convert RPY to a quaternion first, perform the coordinate system conversion on the quaternion, then convert back to RPY (we'll cover quaternion conversion next).

Quaternions represent orientation as a rotation in 3D space, so converting between ENU and NED requires accounting for the fixed coordinate transformation between the two systems.

First, let's define the rotation matrix that maps ENU vectors to NED vectors (we already know this from the vector conversion):

R_NED_from_ENU = [
    [0, 1, 0],  # Map ENU X → NED Y
    [1, 0, 0],  # Map ENU Y → NED X
    [0, 0, -1]  # Map ENU Z → -NED Z
]

This matrix has a determinant of 1, so it's a valid rotation (not a reflection). It corresponds to a 180° rotation around the axis (1,1,0) (the diagonal between East and North directions).

The unit quaternion representing this rotation is:
q_rot = (0.0, √2/2, √2/2, 0.0) (in (w, x, y, z) order, where w is the scalar component).

To convert a quaternion from ENU to NED:

• We apply the coordinate system rotation to the ENU quaternion via quaternion multiplication: q_NED = q_rot * q_ENU
• Quaternion multiplication combines rotations: here, we first apply the ENU orientation, then rotate to map ENU to NED (order matters—left multiplication applies the rotation first).

To convert from NED to ENU:

• We use the inverse of q_rot (which is its conjugate, since it's a unit quaternion): q_ENU = q_rot_conj * q_NED
• The conjugate of q_rot is (0.0, -√2/2, -√2/2, 0.0) (flip the signs of the imaginary components x, y, z).

Let's turn these principles into code. We'll use basic Python (with math for square roots) and avoid external libraries unless needed.

Vector Conversion Functions #

def enu_to_ned(v_enu):
    """Convert ENU vector [East, North, Up] to NED [North, East, Down]."""
    return [v_enu[1], v_enu[0], -v_enu[2]]

def ned_to_enu(v_ned):
    """Convert NED vector [North, East, Down] to ENU [East, North, Up]."""
    return [v_ned[1], v_ned[0], -v_ned[2]]

# Test your example
v_enu_example = [5, 2, 1]
v_ned_example = enu_to_ned(v_enu_example)
print(f"ENU {v_enu_example} → NED {v_ned_example}")  # Output: ENU [5, 2, 1] → NED [2, 5, -1]

Quaternion Conversion Functions #

import math

# Predefined rotation quaternion (w, x, y, z) for ENU ↔ NED transformation
Q_ROT = (0.0, math.sqrt(2)/2, math.sqrt(2)/2, 0.0)

def quat_mult(q1, q2):
    """Multiply two quaternions q1=(w1,x1,y1,z1) and q2=(w2,x2,y2,z2)."""
    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 quat_conj(q):
    """Compute the conjugate of a quaternion (w,x,y,z)."""
    w, x, y, z = q
    return (w, -x, -y, -z)

def enu_quat_to_ned(q_enu):
    """Convert an ENU-oriented quaternion to NED-oriented."""
    return quat_mult(Q_ROT, q_enu)

def ned_quat_to_enu(q_ned):
    """Convert a NED-oriented quaternion to ENU-oriented."""
    q_rot_conj = quat_conj(Q_ROT)
    return quat_mult(q_rot_conj, q_ned)

# Test: Convert identity quaternion (no rotation) from ENU to NED
q_enu_identity = (1.0, 0.0, 0.0, 0.0)
q_ned_result = enu_quat_to_ned(q_enu_identity)
print(f"ENU identity quaternion → NED: {q_ned_result}")
# Output: (0.0, 0.7071067811865476, 0.7071067811865476, 0.0) (matches Q_ROT)

RPY Conversion (Optional) #

If you need to convert RPY vectors, here's a helper to convert RPY ↔ quaternion (assuming XYZ fixed-axis order), then use the quaternion conversion functions above:

def rpy_to_quat(roll, pitch, yaw):
    """Convert RPY (XYZ fixed-axis) to quaternion (w,x,y,z)."""
    cr = math.cos(roll / 2)
    sr = math.sin(roll / 2)
    cp = math.cos(pitch / 2)
    sp = math.sin(pitch / 2)
    cy = math.cos(yaw / 2)
    sy = math.sin(yaw / 2)

    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 quat_to_rpy(q):
    """Convert quaternion (w,x,y,z) to RPY (XYZ fixed-axis)."""
    w, x, y, z = q
    roll = math.atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
    pitch = math.asin(2*(w*y - z*x))
    yaw = math.atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
    return (roll, pitch, yaw)

# Example: Convert ENU RPY to NED RPY
rpy_enu = (0.1, 0.2, 0.3)
q_enu = rpy_to_quat(*rpy_enu)
q_ned = enu_quat_to_ned(q_enu)
rpy_ned = quat_to_rpy(q_ned)
print(f"ENU RPY {rpy_enu} → NED RPY {rpy_ned}")

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