企业官网建设流程全解析

前言

把旋转矩阵转换成欧拉角，是机器人、图形学、惯性导航里最常见的坐标变换操作之一。看起来只是几个atan2和asin，但这段代码几乎是最容易出错的一类：同一份「正确」的公式，换一个旋转顺序、换一种内旋外旋约定，算出来的三个角就完全不同；输入矩阵有微小数值误差时，asin的参数可能略大于 1，直接返回 NaN；接近万向锁时，三个角里有两个无法独立确定，代码却仍然给出一个看似合理的解。

所以本文的第一句话是：脱离约定谈欧拉角没有意义。动手之前必须明确三件事——旋转顺序（先绕哪个轴转）、内旋还是外旋（绕固定轴还是绕自身轴）、主动还是被动（旋转物体还是旋转坐标系）。这三件事确定之后，公式才是唯一的。

本文以「先绕 X、再绕 Y、最后绕 Z 的内旋」为例，也就是等价的「外旋 Z-Y-X」，推导矩阵元素与角度的对应关系，处理万向锁，并分别给出 C++ 与 Python 的实现。C++ 部分以 C++17 为基准，Python 部分以 Python 3 加 NumPy 为基准。

一、约定：顺序、内旋与外旋

设三个角分别是 roll（绕 X）、pitch（绕 Y）、yaw（绕 Z）。内旋（intrinsic）指每次旋转都绕「旋转后的新坐标轴」，外旋（extrinsic）指每次都绕「原始固定坐标轴」。两者有一个著名的等价关系：内旋 X-Y-Z 等价于外旋 Z-Y-X，因为先绕新 X 转、再绕新 Y 转，等价于先绕固定 Z 转、再绕固定 Y 转、最后绕固定 X 转的组合顺序反过来。

本文采用内旋 X-Y-Z，对应的矩阵乘法顺序是从右往左：

R = Rz(yaw) * Ry(pitch) * Rx(roll)

这里的R是把物体坐标系中的向量变换到参考坐标系的旋转矩阵，作用在列向量左侧。若你的场景是把参考系向量变到物体坐标系，或者用的是行向量右乘，那么要用的其实是R的转置，公式会整体变化。

另一个容易混淆的点是「主动旋转」与「被动旋转」。主动旋转转动的是物体，被动旋转转动的是坐标系，两者互为逆旋转，矩阵互为转置。很多库的文档只写「roll-pitch-yaw」，不写清楚是哪一种，照抄公式就会得到方向相反的角。

二、元素对照与三个公式

把三个基本旋转矩阵相乘，展开后得到：

[ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr ]
R = [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr ]
[ -sp, cp*sr, cp*cr ]

其中sr、cr是 roll 的正弦余弦，sp、cp对应 pitch，sy、cy对应 yaw。

观察第三行第一列：R[2][0] = -sin(pitch)。这一个元素就确定了 pitch：

pitch = asin(-R[2][0])

再看第一行第三列与第二行第三列：它们分别是cp*sr与cp*cr，两者的比值消去了cp，于是：

roll = atan2(R[2][1], R[2][2])

同理，第一列的前两个元素是cy*cp与sy*cp，比值为tan(yaw)：

yaw = atan2(R[1][0], R[0][0])

这三个式子构成了 ZYX 顺序的标准解法。注意atan2(y, x)的参数顺序：第一个是「对边」，第二个是「邻边」，写反了会得到余角，这是最常见的低级错误之一。

三个角的值域也随之确定：pitch 在负九十度到正九十度之间（因为asin的值域就是如此），roll 与 yaw 在负一百八十度到正一百八十度之间（atan2的值域）。如果你的应用要求 yaw 落在 0 到 360 度，需要额外做一次归一化。

三、万向锁与分支处理

当cos(pitch)趋近于 0，也就是 pitch 接近正负九十度时，R[0][0]与R[1][0]同时趋近于 0，atan2(0, 0)在多数实现上返回 0，看起来像一个合法结果，实际上此时 roll 与 yaw 已经无法独立确定：矩阵只在「roll 与 yaw 的和」或「差」上有意义。

从矩阵看得很清楚。pitch 等于正九十度时，第三行变成[-1, 0, 0]，第一行退化为[0, sin(roll - yaw), cos(roll - yaw)]，只依赖两者之差；pitch 等于负九十度时，第一行退化为[0, -sin(roll + yaw), -cos(roll + yaw)]，只依赖两者之和。这就是万向锁（gimbal lock）的代数表现。

工程上的处理约定是：把其中一个角固定为 0（通常固定 roll），把全部旋转归到另一个角上。此时：

roll = 0
yaw = atan2(-R[0][1], R[1][1])
pitch = atan2(-R[2][0], sy) // sy 趋近 0，结果为 ±π/2

这两个奇异点用同一个 yaw 公式即可，因为代入后都能化简成atan2(sin(yaw), cos(yaw))。推导过程不需要背，只要承认「此时只剩一个自由度」，然后从矩阵里挑一对能唯一确定它的元素。

判别条件用sqrt(R[0][0]² + R[1][0]²) > eps，其中eps取 1e-12 左右。这里不能用R[2][0]是否等于正负 1 来判断，因为浮点误差会让它变成 1.0000000000000002，等值比较永远不成立。

四、C++ 实现

// 适用于 C++17
#include <array>
#include <cmath>
#include <iostream>

using Mat3 = std::array<std::array<double, 3>, 3>;

struct EulerZYX {
double roll; // 绕 X，弧度
double pitch; // 绕 Y，弧度
double yaw; // 绕 Z，弧度
};

EulerZYX matrix_to_euler_zyx(const Mat3& R) {
const double eps = 1e-12;
const double sy = std::hypot(R[0][0], R[1][0]); // |cos(pitch)|

EulerZYX e{};
if (sy > eps) {
e.roll = std::atan2(R[2][1], R[2][2]);
e.pitch = std::atan2(-R[2][0], sy);
e.yaw = std::atan2(R[1][0], R[0][0]);
} else {
// 万向锁：固定 roll 为 0，旋转全部归到 yaw
e.roll = 0.0;
e.pitch = std::atan2(-R[2][0], sy);
e.yaw = std::atan2(-R[0][1], R[1][1]);
}
return e;
}

Mat3 euler_zyx_to_matrix(double roll, double pitch, double yaw) {
const double cr = std::cos(roll);
const double sr = std::sin(roll);
const double cp = std::cos(pitch);
const double sp = std::sin(pitch);
const double cy = std::cos(yaw);
const double sy = std::sin(yaw);

Mat3 m{};
m[0] = {cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr};
m[1] = {sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr};
m[2] = {-sp, cp * sr, cp * cr};
return m;
}

int main() {
const double deg = 3.14159265358979323846 / 180.0;
const Mat3 R = euler_zyx_to_matrix(30.0 * deg, 20.0 * deg, 10.0 * deg);
const EulerZYX e = matrix_to_euler_zyx(R);

std::cout << "roll = " << e.roll / deg << '\n';
std::cout << "pitch = " << e.pitch / deg << '\n';
std::cout << "yaw = " << e.yaw / deg << '\n';
return 0;
}

这里用atan2(-R[2][0], sy)而不是asin(-R[2][0])，是一个有意的选择。atan2的参数是两个分量，即使R[2][0]因为误差略微超过 1，结果仍然是有限值，不会返回 NaN；而asin在参数超出定义域时返回 NaN，并可能设置errno。用hypot计算sy还能避免中间结果溢出，比手写sqrt(a*a + b*b)更稳妥。

euler_zyx_to_matrix用来做往返自检：先用已知角度构造矩阵，再反解，比较结果是否接近原值。这是验证这类公式最实用的手段——不需要相信任何一篇博客，只要正反两个方向能对上，约定就是自洽的。

五、Python 实现

同样的逻辑用 NumPy 写出来只有十几行。Python 的math.atan2、math.hypot与 C++ 的std::atan2、std::hypot语义一致，都是弧度制。

# 适用于 Python 3.8+，需要 numpy
import math

import numpy as np


def matrix_to_euler_zyx(R):
R = np.asarray(R, dtype=float)
if R.shape != (3, 3):
raise ValueError("需要 3x3 旋转矩阵")

eps = 1e-12
sy = math.hypot(R[0, 0], R[1, 0]) # |cos(pitch)|

if sy > eps:
roll = math.atan2(R[2, 1], R[2, 2])
pitch = math.atan2(-R[2, 0], sy)
yaw = math.atan2(R[1, 0], R[0, 0])
else:
roll = 0.0
pitch = math.atan2(-R[2, 0], sy)
yaw = math.atan2(-R[0, 1], R[1, 1])
return roll, pitch, yaw


def euler_zyx_to_matrix(roll, pitch, yaw):
cr, sr = math.cos(roll), math.sin(roll)
cp, sp = math.cos(pitch), math.sin(pitch)
cy, sy = math.cos(yaw), math.sin(yaw)
return np.array([
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
[-sp, cp * sr, cp * cr],
])

两个实现之间有一个细节差异值得注意：NumPy 的浮点运算默认是双精度，但矩阵来自外部时可能是float32，np.asarray(..., dtype=float)会在转换时保留已经损失的精度。如果矩阵是传感器直接给出的单精度数据，反解出的角度误差会比双精度大得多，这是数据源的问题，不是公式的问题。

常见坑点

坑点一：不声明旋转顺序就套公式。

❌ 拿到一段 ZYX 的代码，用在 XYZ 顺序的数据上，三个角全部错位。

✅ 先确认顺序与内外旋约定，再选公式；不同顺序的矩阵展开完全不同。

坑点二：asin的参数不做保护。

❌pitch = std::asin(-R[2][0]);当R[2][0]为 -1.0000000000000002 时返回 NaN。

✅ 用atan2(-R[2][0], std::hypot(R[0][0], R[1][0]))，或先std::clamp到[-1, 1]。

坑点三：不处理万向锁。

❌ 直接用atan2(R[2][1], R[2][2])求 roll，在 pitch 为九十度附近得到一个随噪声跳变的值。

✅ 用hypot(R[0][0], R[1][0])判断是否进入奇异区，进入后固定一个角、只求另一个。

坑点四：角度与弧度混用。

❌ 输入是度，却直接传给std::sin；或者把atan2返回的弧度当成度输出。

✅ 全部内部计算用弧度，只在输入输出边界做deg = rad * 180 / π的转换。

坑点五：把内旋当成外旋。

❌ 内旋 X-Y-Z 的数据套用外旋 X-Y-Z 的公式，两个角符号相反。

✅ 记住内旋 X-Y-Z 等价于外旋 Z-Y-X，按对应的矩阵乘法顺序展开。

坑点六：以为任意矩阵都能直接转置求逆。

❌ 对含噪声的矩阵做R.transpose()当作R.inverse()，误差被放大。

✅ 只有正交矩阵才有这个性质；噪声数据应当先做正交化（如 SVD 投影）。

坑点七：用float保存中间结果。

❌ 大角度下float的有效位不足，反解出的角在小数点后几位来回跳动。

✅ 角度计算全程用double，需要单精度时只在最终输出转换。

坑点八：只验证一个方向。

❌ 只写了矩阵到角度的代码，从不检查反解结果能否重建原矩阵。

✅ 写一个euler_to_matrix做往返测试，误差应在 1e-9 量级（双精度）。

总结

旋转矩阵与欧拉角的互转，公式本身只有六行，剩下的全是约定与边界。写完代码后不要急着相信结果，先构造一个已知角度的矩阵做往返，再单独测试 pitch 为正负九十度的情况，最后用一组带噪声的矩阵看输出是否连续。这三步做完，才有资格说这段代码在你的坐标系里是对的。