算法_四元数到姿态角(欧拉角\固定角\轴角)的转换代码python/C++

原创已于 2025-11-21 11:39:42 修改 · 207 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #数据库

于 2025-11-13 09:46:05 首次发布

一、四元数到旋转姿态角

姿态角旋转的类别主要涉及有内旋(欧拉角)和外旋(固定角)，姿态角按照旋转轴是x轴，y轴和z轴的顺序可以通俗的叫做横滚角(roll)、俯仰角(pitch)和航向角(heading/yaw)。

Python代码

import numpy as np
from scipy.spatial.transform import quaternions, euler

def quat_to_attitude_angle(q, type_='sxyz'):
    """
    四元数转指定类型的姿态角（欧拉角/固定角）
    输入01：q - 四元数，格式为 [w, x, y, z]（实部w在前，虚部x,y,z在后）
    输入02：type_ - 旋转类型（必选合法值，默认'sxyz'），格式规则：
            - 首字符：'s'（内旋/欧拉角，旋转轴随刚体转动）、'r'（外旋/固定角，旋转轴固定于世界坐标系）
            - 后三位：旋转顺序（如'xyz'表示x→y→z顺规）
            - 合法值：['sxyz','sxzy','szyx','szxy','syxz','syzx','rxyz','rxzy','ryzx','ryxz','rzxy','rzyx']
    输出：euler_angles - 欧拉角（弧度），顺序与 type_ 的后三位完全一致
          例：type_='sxyz' → 输出 [x轴旋转, y轴旋转, z轴旋转]（内旋）；type_='rzyx' → 输出 [z轴旋转, y轴旋转, x轴旋转]（外旋）
    异常：输入非法时直接抛出 ValueError，明确告知错误原因
    """
    # invaild string check
    VALID_ROTATION_TYPES = {
        'sxyz', 'sxzy', 'szyx', 'szxy', 'syxz', 'syzx',
        'rxyz', 'rxzy', 'ryzx', 'ryxz', 'rzxy', 'rzyx'
    }
    if type_ not in VALID_ROTATION_TYPES:
        raise ValueError(
            f"旋转类型 '{type_}' 非法！\n"
            f"合法类型需满足：首字符's'（内旋）或'r'（外旋），后三位为旋转顺序，共12种：\n"
            f"{sorted(VALID_ROTATION_TYPES)}"
        )
    
    # normalization matrix
    q = np.asarray(q, dtype=np.float64)  # 统一转为float64数组
    if q.shape != (4,):
        raise ValueError(f"四元数需为4个元素的列表/数组，当前输入长度：{len(q)}")
    q_norm = np.linalg.norm(q)
    if q_norm < 1e-6:  # 避免除以0（无效四元数）
        raise ValueError("输入四元数全为0，无法表示姿态")
    q_normalized = q / q_norm  # 归一化后再计算
    
    # core algorithm
    rotation_matrix = quaternions.quat2mat(q_normalized)
    euler_angles = euler.mat2euler(rotation_matrix, axes=type_)
    
    return euler_angles

C++代码

#include <Eigen/Geometry>
#include <string>
#include <unordered_map>
#include <stdexcept>

// 将旋转序列名称映射到 Eigen 的 (i,j,k) 参数
std::unordered_map<std::string, std::array<int, 3>> getRotationSequenceMap() {
    return {
        // 内旋（intrinsic）命名：r = rotating frame
        {"rzyx", {2, 1, 0}}, // ZYX
        {"rxyz", {0, 1, 2}}, // XYZ
        {"ryzx", {1, 2, 0}}, // YZX
        {"rzxy", {2, 0, 1}}, // ZXY
        {"rxzy", {0, 2, 1}}, // XZY
        {"ryxz", {1, 0, 2}}, // YXZ

        // 外旋（extrinsic）命名：s = static/world frame
        // sABC ≡ rCBA → same as rCBA
        {"sxyz", {2, 1, 0}}, // ≡ rzyx
        {"szyx", {0, 1, 2}}, // ≡ rxyz
        {"sxzy", {1, 2, 0}}, // ≡ ryzx
        {"syxz", {2, 0, 1}}, // ≡ rzxy
        {"syzx", {0, 2, 1}}, // ≡ rxzy
        {"szxy", {1, 0, 2}}  // ≡ ryxz
    };
}

Eigen::Vector3d quatToEuler(const double q[4], const std::string& sequence) {
    // 解析四元数 [w, x, y, z]
    Eigen::Quaterniond quat(q[0], q[1], q[2], q[3]);
    quat.normalize();

    // 获取旋转顺序映射
    static const auto seqMap = getRotationSequenceMap();
    auto it = seqMap.find(sequence);
    if (it == seqMap.end()) {
        throw std::invalid_argument("Unsupported rotation sequence: " + sequence);
    }

    auto [i, j, k] = it->second;
    Eigen::Matrix3d R = quat.toRotationMatrix();
    return R.eulerAngles(i, j, k); // 返回 [angle_k, angle_j, angle_i] 对应旋转顺序
}

// 兼容旧接口（可选）
void quart_to_atti_angle(const double q[4], const std::string& type,
                         double& roll, double& pitch, double& yaw) {
    Eigen::Vector3d euler = quatToEuler(q, type);
    roll  = euler.x();   // 对应第一个旋转轴（如 X for rxyz/szyx）
    pitch = euler.y();   // 第二个轴（如 Y）
    yaw   = euler.z();   // 第三个轴（如 Z）
}

二、四元数到轴角(分别包含了轴和角)转换

Python 代码

import numpy as np

def quat_to_axis_angle(q):
    """
    Convert a unit quaternion [w, x, y, z] to axis-angle representation.
    Returns:
        axis: np.array of shape (3,), unit vector (undefined if angle=0)
        angle: float, rotation angle in radians, in range [0, pi]
    """
    w, x, y, z = q
    # Ensure unit norm
    norm = np.linalg.norm([w, x, y, z])
    if norm == 0:
        raise ValueError("Zero-norm quaternion")
    w, x, y, z = w / norm, x / norm, y / norm, z / norm
    # Enforce positive scalar part for uniqueness (theta in [0, pi])
    if w < 0:
        w, x, y, z = -w, -x, -y, -z
    angle = 2 * np.arccos(np.clip(w, -1.0, 1.0))
    sin_half_angle = np.sqrt(1.0 - w * w)  # = sqrt(x^2 + y^2 + z^2)
    if sin_half_angle < 1e-8:              # Near zero rotation
        axis = np.array([1.0, 0.0, 0.0])   # arbitrary, or could return [0,0,0]
    else:
        axis = np.array([x, y, z]) / sin_half_angle
    return axis, angle

C++ 代码

#include <Eigen/Geometry>
#include <iostream>
#include <cmath>
#include <stdexcept>

/**
 * 将单位四元数转换为轴角表示
 * @param q 四元数 [w, x, y, z]
 * @param axis 输出：旋转轴（单位向量）
 * @param angle 输出：旋转角（弧度，范围 [0, π]）
 */
void quaternionToAxisAngle(const double q[4], Eigen::Vector3d& axis, double& angle) {
    double w = q[0];
    double x = q[1];
    double y = q[2];
    double z = q[3];

    // 构造并归一化四元数（确保数值稳定）
    Eigen::Quaterniond quat(w, x, y, z);
    quat.normalize();

    // 方法1：直接使用 Eigen 内置函数（推荐）
    Eigen::AngleAxisd aa(quat);
    axis  = aa.axis();   // 单位向量
    angle = aa.angle();  // 弧度 ∈ [0, π]

    // 特殊情况：零旋转（angle ≈ 0）
    // 此时 axis 未定义，Eigen 通常返回 (1,0,0)，可按需处理
}

// 重载：返回 std::pair（更现代的接口）
std::pair<Eigen::Vector3d, double> quatToAxisAngle(const double q[4]) {
    Eigen::Vector3d axis;
    double angle;
    quaternionToAxisAngle(q, axis, angle);
    return {axis, angle};
}

三、姿态角到四元数转换

Python代码

import numpy as np
from scipy.spatial.transform import Rotation as R

def attitude_angle_to_quat(angles, type_='sxyz', is_degree=False):
    """
    将指定类型的姿态角转换为四元数 [w, x, y, z]
    参数:
        angles: 长度为3的数组，旋转角（顺序与 type_ 后三位一致）
        type_: 旋转类型，如 'sxyz', 'rzyx' 等（共12种）
        is_degree: 若为 True，angles 为角度；否则为弧度（默认）
    返回:
        [w, x, y, z] 格式的单位四元数（numpy array）
    """
    VALID_ROTATION_TYPES = {
        'sxyz', 'sxzy', 'szyx', 'szxy', 'syxz', 'syzx',
        'rxyz', 'rxzy', 'ryzx', 'ryxz', 'rzxy', 'rzyx'
    }
    if type_ not in VALID_ROTATION_TYPES:
        raise ValueError(
            f"旋转类型 '{type_}' 非法！合法类型共12种：\n{sorted(VALID_ROTATION_TYPES)}"
        )
    angles = np.asarray(angles, dtype=np.float64)
    if angles.shape != (3,):
        raise ValueError(f"姿态角必须是长度为3的数组，当前输入形状：{angles.shape}")
    seq = type_[1:].lower() if type_[0] == 's' else type_[1:].upper()    
    rotation = R.from_euler(seq, angles, degrees=is_degree)
    quat_xyzw = rotation.as_quat()
    return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]])

C++代码

#include <Eigen/Geometry>
#include <string>
#include <unordered_map>
#include <stdexcept>
#include <iostream>

// 复用你已有的映射表（可放在头文件中共享）
std::unordered_map<std::string, std::array<int, 3>> getRotationSequenceMap() {
    return {
        {"rzyx", {2, 1, 0}}, {"rxyz", {0, 1, 2}}, {"ryzx", {1, 2, 0}},
        {"rzxy", {2, 0, 1}}, {"rxzy", {0, 2, 1}}, {"ryxz", {1, 0, 2}},
        {"sxyz", {2, 1, 0}}, {"szyx", {0, 1, 2}}, {"sxzy", {1, 2, 0}},
        {"syxz", {2, 0, 1}}, {"syzx", {0, 2, 1}}, {"szxy", {1, 0, 2}}
    };
}

/**
 * 将姿态角（欧拉角/固定角）转换为四元数
 * @param euler 输入角度 [roll, pitch, yaw]（弧度）
 *              roll: 绕 X 轴, pitch: 绕 Y 轴, yaw: 绕 Z 轴
 * @param sequence 旋转顺序，如 "rzyx", "sxyz" 等
 * @return 单位四元数 [w, x, y, z]
 */
Eigen::Quaterniond eulerToQuat(const Eigen::Vector3d& euler, const std::string& sequence) {
    static const auto seqMap = getRotationSequenceMap();
    auto it = seqMap.find(sequence);
    if (it == seqMap.end()) {
        throw std::invalid_argument("Unsupported rotation sequence: " + sequence);
    }

    double roll  = euler.x();   // α: X
    double pitch = euler.y();   // β: Y
    double yaw   = euler.z();   // γ: Z

    // 根据 sequence 决定如何组合旋转
    // 注意：所有情况都可归结为某种内旋顺序的矩阵乘积
    Eigen::Matrix3d R;

    if (sequence == "rzyx" || sequence == "sxyz") {
        // ZYX intrinsic (or XYZ extrinsic): R = Rz(yaw) * Ry(pitch) * Rx(roll)
        R = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
          * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());
    }
    else if (sequence == "rxyz" || sequence == "szyx") {
        // XYZ intrinsic (or ZYX extrinsic): R = Rx(roll) * Ry(pitch) * Rz(yaw)
        R = Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ());
    }
    else if (sequence == "ryzx" || sequence == "sxzy") {
        // YZX intrinsic: R = Ry(pitch) * Rz(yaw) * Rx(roll)
        R = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
          * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());
    }
    else if (sequence == "rzxy" || sequence == "syxz") {
        // ZXY intrinsic: R = Rz(yaw) * Rx(roll) * Ry(pitch)
        R = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
          * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY());
    }
    else if (sequence == "rxzy" || sequence == "syzx") {
        // XZY intrinsic: R = Rx(roll) * Rz(yaw) * Ry(pitch)
        R = Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
          * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY());
    }
    else if (sequence == "ryxz" || sequence == "szxy") {
        // YXZ intrinsic: R = Ry(pitch) * Rx(roll) * Rz(yaw)
        R = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ());
    }
    else {
        // 理论上不会走到这里（前面已检查）
        throw std::runtime_error("Unexpected rotation sequence");
    }

    return Eigen::Quaterniond(R).normalized();
}

// 兼容旧接口：输入 roll/pitch/yaw（弧度），输出四元数数组 [w,x,y,z]
void atti_angle_to_quart(double roll, double pitch, double yaw,
                         const std::string& type, double q_out[4]) {
    Eigen::Vector3d euler(roll, pitch, yaw);
    Eigen::Quaterniond q = eulerToQuat(euler, type);

    q_out[0] = q.w();
    q_out[1] = q.x();
    q_out[2] = q.y();
    q_out[3] = q.z();
}

int main() {
    // 绕 Z 轴 90°: roll=0, pitch=0, yaw=π/2
    double roll = 0.0, pitch = 0.0, yaw = M_PI / 2.0;
    double q[4];

    atti_angle_to_quart(roll, pitch, yaw, "rzyx", q);
    std::cout << "Quat [w,x,y,z]: "
              << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << "\n";

    // 验证：应接近 [0.7071, 0, 0, 0.7071]
    return 0;
}

四、轴角到四元数的转换

Python 代码

import numpy as np

def axis_angle_to_quaternion(axis, angle, is_degree=False):
    """
    将轴角表示（过原点的旋转轴 + 旋转角度）转换为四元数（w, x, y, z）
    参数:
        axis: 旋转轴向量，numpy数组或列表，格式为 [x, y, z]（过原点）
        angle: 旋转角度（默认弧度制；若为角度制，设置 is_degree=True）
        is_degree: 是否为角度制输入（默认False，即弧度制）
    返回:
        quat: 四元数 numpy 数组，格式为 [w, x, y, z]（实部在前，虚部在后，单位四元数）
    异常:
        ValueError: 若旋转轴为零向量（无法归一化），抛出异常
    """
    # 处理角度单位：角度制 → 弧度制
    if is_degree:
        angle_rad = np.radians(angle)
    else:
        angle_rad = angle
    # 转换轴为numpy数组，并归一化（旋转轴必须是单位向量）
    axis = np.asarray(axis, dtype=np.float64)
    axis_norm = np.linalg.norm(axis)  # 计算轴向量的模长
    # 检查轴向量是否为零向量（无法归一化）
    if axis_norm < 1e-10:
        raise ValueError("旋转轴不能是零向量（[0,0,0]），无法进行归一化！")
    # 归一化轴向量（单位化）
    axis_unit = axis / axis_norm
    # 按公式计算四元数
    w = np.cos(angle_rad / 2.0)
    x = axis_unit[0] * np.sin(angle_rad / 2.0)
    y = axis_unit[1] * np.sin(angle_rad / 2.0)
    z = axis_unit[2] * np.sin(angle_rad / 2.0)
    # 组合为四元数并返回（已保证是单位四元数）
    quat = np.array([w, x, y, z])
    return quat

C++代码

#include <Eigen/Geometry>
#include <iostream>
#include <cmath>
#include <stdexcept>

/**
 * 将轴角表示转换为单位四元数
 * @param axis 输入：旋转轴（不要求单位长度，函数内部会归一化）
 * @param angle 输入：旋转角（弧度，建议 ∈ [0, 2π]，但任意实数均可）
 * @param q_out 输出：四元数 [w, x, y, z]
 */
void axisAngleToQuaternion(const Eigen::Vector3d& axis, double angle, double q_out[4]) {
    // 处理零向量轴（避免除零）
    double axis_norm = axis.norm();
    if (axis_norm < 1e-12) {
        if (std::abs(angle) < 1e-12) {
            // 零旋转：返回单位四元数
            q_out[0] = 1.0;
            q_out[1] = q_out[2] = q_out[3] = 0.0;
        } else {
            // 无效输入：非零角度但零轴
            throw std::invalid_argument("Non-zero angle with zero rotation axis is undefined.");
        }
        return;
    }

    // 归一化旋转轴
    Eigen::Vector3d unit_axis = axis / axis_norm;

    // 计算半角
    double half_angle = angle * 0.5;
    double sin_half = std::sin(half_angle);
    double cos_half = std::cos(half_angle);

    // 构造四元数分量
    q_out[0] = cos_half;                     // w
    q_out[1] = sin_half * unit_axis.x();     // x
    q_out[2] = sin_half * unit_axis.y();     // y
    q_out[3] = sin_half * unit_axis.z();     // z

    // 可选：显式归一化（理论上已单位化，但浮点误差可能累积）
    double norm = std::sqrt(
        q_out[0]*q_out[0] + q_out[1]*q_out[1] +
        q_out[2]*q_out[2] + q_out[3]*q_out[3]
    );
    if (norm > 1e-12) {
        q_out[0] /= norm;
        q_out[1] /= norm;
        q_out[2] /= norm;
        q_out[3] /= norm;
    }
}

// 重载：使用原生 double[3] 表示轴（兼容 C 风格）
void axisAngleToQuaternion(const double axis[3], double angle, double q_out[4]) {
    Eigen::Vector3d ax(axis[0], axis[1], axis[2]);
    axisAngleToQuaternion(ax, angle, q_out);
}

// 现代 C++ 接口：返回 Eigen::Quaterniond
Eigen::Quaterniond axisAngleToQuat(const Eigen::Vector3d& axis, double angle) {
    double q[4];
    axisAngleToQuaternion(axis, angle, q);
    return Eigen::Quaterniond(q[0], q[1], q[2], q[3]).normalized();
}

// 兼容 ROS / geometry_msgs 风格（可选）
Eigen::Quaterniond axisAngleToQuat(double ax, double ay, double az, double angle) {
    return axisAngleToQuat(Eigen::Vector3d(ax, ay, az), angle);
}

int main() {
    // 示例1：绕 Z 轴旋转 90° (π/2)
    Eigen::Vector3d axis(0, 0, 1);
    double angle = M_PI / 2.0;
    double q[4];

    axisAngleToQuaternion(axis, angle, q);
    std::cout << "Quat [w,x,y,z]: "
              << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << "\n";

    // 验证：应 ≈ [0.7071, 0, 0, 0.7071]

    // 示例2：使用原生数组
    double axis_arr[3] = {1, 1, 0};
    double angle2 = M_PI; // 180°
    double q2[4];
    axisAngleToQuaternion(axis_arr, angle2, q2);
    std::cout << "180° around (1,1,0): "
              << q2[0] << ", " << q2[1] << ", " << q2[2] << ", " << q2[3] << "\n";

    // 示例3：现代接口
    auto q3 = axisAngleToQuat(0, 0, 1, M_PI); // 绕 Z 转 180°
    std::cout << "Modern: w=" << q3.w() << ", x=" << q3.x()
              << ", y=" << q3.y() << ", z=" << q3.z() << "\n";

    return 0;
}

五、四元数到姿态变化矩阵

在这里插入图片描述

Python 转换代码

def quat_to_rotation_matrix(q):
    w, x, y, z = q
    return np.array([
        [1 - 2*y*y - 2*z*z,     2*x*y - 2*w*z,     2*x*z + 2*w*y],
        [    2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z,     2*y*z - 2*w*x],
        [    2*x*z - 2*w*y,     2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
    ])

或者引用函数库的表示

import numpy as np
from scipy.spatial.transform import Rotation as R

q_wxyz = [np.sqrt(2)/2, 0, 0, np.sqrt(2)/2]  # 绕 Z 轴旋转 90° # 假设你的四元数是 [w, x, y, z] 格式（常见于数学、Eigen、ROS）
q_xyzw = [q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]] # 转换为 scipy 所需的 [x, y, z, w]
rot = R.from_quat(q_xyzw) # 创建 Rotation 对象
rotation_matrix = rot.as_matrix() # 获取 3×3 旋转矩阵
print("旋转矩阵 R =\n", rotation_matrix)

C++转换代码

#include <Eigen/Geometry>
#include <iostream>

/**
 * 将单位四元数转换为 3x3 旋转矩阵（姿态变化矩阵）
 * @param q 四元数数组 [w, x, y, z]
 * @return 3x3 旋转矩阵 R ∈ SO(3)
 */
Eigen::Matrix3d quaternionToRotationMatrix(const double q[4]) {
    // 构造 Eigen 四元数 (w, x, y, z)
    Eigen::Quaterniond quat(q[0], q[1], q[2], q[3]);
    
    // 自动归一化（确保正交性）
    quat.normalize();
    
    // 转换为旋转矩阵
    return quat.toRotationMatrix();
}

// 可选：重载版本，支持 std::array 或 vector
Eigen::Matrix3d quaternionToRotationMatrix(const std::array<double, 4>& q) {
    return quaternionToRotationMatrix(q.data());
}

int main() {
    // 示例：绕 Z 轴旋转 90°
    // q = [cos(π/4), 0, 0, sin(π/4)] ≈ [0.7071, 0, 0, 0.7071]
    double q[4] = {0.70710678118, 0.0, 0.0, 0.70710678118};

    Eigen::Matrix3d R = quaternionToRotationMatrix(q);

    std::cout << "Rotation Matrix:\n" << R << "\n";

    // 验证：应接近
    // [ 0 -1  0 ]
    // [ 1  0  0 ]
    // [ 0  0  1 ]

    return 0;
}

六、姿态变化矩阵到四元数

在这里插入图片描述

Python转换代码

import numpy as np

def rotation_matrix_to_quaternion(R):
    """
    将 3x3 旋转矩阵转换为四元数 [w, x, y, z]
    :param R: np.ndarray, shape (3, 3)
    :return: np.ndarray, shape (4,) —— [w, x, y, z]
    """
    assert R.shape == (3, 3), "Input must be a 3x3 matrix"
    # 确保是旋转矩阵（可选：正交化）
    # R = (R + R.T) / 2  # 对称化（应对数值误差）
    r00, r01, r02 = R[0]
    r10, r11, r12 = R[1]
    r20, r21, r22 = R[2]
    trace = r00 + r11 + r22
    if trace > 0:
        s = np.sqrt(trace + 1.0) * 2  # s = 4 * w
        w = 0.25 * s
        x = (r21 - r12) / s
        y = (r02 - r20) / s
        z = (r10 - r01) / s
    elif (r00 > r11) and (r00 > r22):
        s = np.sqrt(1.0 + r00 - r11 - r22) * 2  # s = 4 * x
        w = (r21 - r12) / s
        x = 0.25 * s
        y = (r01 + r10) / s
        z = (r02 + r20) / s
    elif r11 > r22:
        s = np.sqrt(1.0 + r11 - r00 - r22) * 2  # s = 4 * y
        w = (r02 - r20) / s
        x = (r01 + r10) / s
        y = 0.25 * s
        z = (r12 + r21) / s
    else:
        s = np.sqrt(1.0 + r22 - r00 - r11) * 2  # s = 4 * z
        w = (r10 - r01) / s
        x = (r02 + r20) / s
        y = (r12 + r21) / s
        z = 0.25 * s
    q = np.array([w, x, y, z])
    # 归一化（应对数值误差）
    return q / np.linalg.norm(q)

使用 scipy 的简洁方法

from scipy.spatial.transform import Rotation as R
# 假设 R_mat 是你的 3x3 旋转矩阵
R_mat = np.array([[0, -1, 0],
                  [1,  0, 0],
                  [0,  0, 1]])
# 创建 Rotation 对象
rot = R.from_matrix(R_mat)
# 获取四元数（注意：scipy 输出为 [x, y, z, w]）
q_xyzw = rot.as_quat()
# 转换为 [w, x, y, z]
q_wxyz = np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]])
print("四元数 [w, x, y, z] =", q_wxyz)

C++代码

#include <Eigen/Geometry>
#include <iostream>
#include <cmath>
#include <stdexcept>

/**
 * 将 3x3 旋转矩阵（姿态矩阵）转换为单位四元数
 * @param R 输入的 3x3 旋转矩阵
 * @param q_out 输出四元数数组 [w, x, y, z]
 * @param check_validity 是否检查矩阵有效性（默认 true）
 */
void rotationMatrixToQuaternion(const Eigen::Matrix3d& R, double q_out[4], bool check_validity = true) {
    if (check_validity) {
        // 检查是否近似正交：R^T * R ≈ I
        Eigen::Matrix3d RtR = R.transpose() * R;
        Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
        if ((RtR - I).norm() > 1e-6) {
            throw std::invalid_argument("Input matrix is not a valid rotation matrix (not orthogonal).");
        }
        // 检查行列式是否为 +1（排除反射）
        if (std::abs(R.determinant() - 1.0) > 1e-6) {
            throw std::invalid_argument("Input matrix has determinant != +1 (not a proper rotation).");
        }
    }

    // 使用 Eigen 构造四元数
    Eigen::Quaterniond quat(R);
    quat.normalize(); // 确保单位长度（应对数值误差）

    q_out[0] = quat.w();
    q_out[1] = quat.x();
    q_out[2] = quat.y();
    q_out[3] = quat.z();
}

// 重载：支持 std::array 输出（现代 C++ 风格）
std::array<double, 4> rotationMatrixToQuaternion(const Eigen::Matrix3d& R, bool check_validity = true) {
    std::array<double, 4> q;
    rotationMatrixToQuaternion(R, q.data(), check_validity);
    return q;
}

// 重载：直接返回 Eigen::Quaterniond（最简洁）
Eigen::Quaterniond matrixToQuat(const Eigen::Matrix3d& R) {
    return Eigen::Quaterniond(R).normalized();
}

int main() {
    // 构造一个绕 Z 轴 90° 的旋转矩阵
    Eigen::Matrix3d R;
    R <<  0.0, -1.0,  0.0,
          1.0,  0.0,  0.0,
          0.0,  0.0,  1.0;

    // 方法1：输出到数组
    double q1[4];
    try {
        rotationMatrixToQuaternion(R, q1);
        std::cout << "Quat [w,x,y,z]: "
                  << q1[0] << ", " << q1[1] << ", " << q1[2] << ", " << q1[3] << "\n";
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
    }

    // 方法2：使用 std::array
    auto q2 = rotationMatrixToQuaternion(R);
    std::cout << "As array: w=" << q2[0] << ", x=" << q2[1] << ", y=" << q2[2] << ", z=" << q2[3] << "\n";

    // 方法3：直接获取 Quaterniond
    Eigen::Quaterniond q3 = matrixToQuat(R);
    std::cout << "Eigen quat: w=" << q3.w() << ", x=" << q3.x()
              << ", y=" << q3.y() << ", z=" << q3.z() << "\n";

    return 0;
}