1. 姿态估计概述：什么是姿态？

大家好，我是你们这门课的老朋友。今天咱们聊聊姿态估计最基础的东西——到底什么是姿态？

说实话，我入行那会儿，第一次听到「姿态」这个词，脑子里蹦出来的是芭蕾舞演员的姿势。后来做惯导项目才明白，工程上的姿态，说白了就是：一个物体在三维空间里「朝哪边看」。

你想想看，一架无人机在空中，它不光要知道自己在哪里（位置），还得知道自己机头朝哪、机翼是否水平。这个「朝向」就是姿态。再比如你手机里的陀螺仪，它也在实时估算姿态，不然横竖屏切换怎么来的？

嗯，这里要注意：姿态不包含位置信息。位置是平移，姿态是旋转。两者合起来叫位姿，那是后话了。

1.1 欧拉角——最直观，但最坑

欧拉角是我个人习惯用来做可视化调试的。它用三个角度来描述旋转：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。你可以想象成飞机在空中：

• 横滚（Roll）：绕机头轴线旋转，像飞机侧身转弯
• 俯仰（Pitch）：绕机翼轴线旋转，像飞机抬头低头
• 偏航（Yaw）：绕垂直轴线旋转，像飞机左右转向

听起来很直观对吧？但我在项目中遇到过一个大坑——万向锁（Gimbal Lock）。当俯仰角达到±90°时，横滚和偏航会变成同一个自由度，你丢失了一个控制维度。这就像你开车时方向盘突然卡住，只能直行，多吓人。

1.2 旋转矩阵——数学上完美，计算上沉重

旋转矩阵是一个3×3的正交矩阵，行列式为+1。它把向量从一个坐标系变换到另一个坐标系。比如你有一个向量在机体坐标系下是 v_body，想知道它在导航坐标系下长什么样？乘上旋转矩阵就行：

v_nav = R * v_body

旋转矩阵的好处是：没有奇点，可以连续旋转。但坏处也很明显——9个元素，每次更新都要做大量乘法。我在做嵌入式IMU解算时，用旋转矩阵跑一次姿态更新，CPU占用率直接飙到30%。

而且旋转矩阵必须保持正交性，数值误差会让它慢慢「变形」，需要定期重新正交化。这就像你每天都要把歪了的相框扶正，烦不烦？

表示方法	优点	缺点
欧拉角	直观，3个参数	万向锁，不适合插值
旋转矩阵	无奇点，可连续旋转	9个参数，计算量大，需正交化
四元数	4个参数，无奇点，计算快	不够直观，需归一化

1.3 四元数——工程师的瑞士军刀

四元数是我最推荐大家在卡尔曼滤波里用的姿态表示方式。它由1个实部和3个虚部组成：

q = w + x*i + y*j + z*k

其中 w 是实部，x、y、z 是虚部。而且满足 w² + x² + y² + z² = 1（归一化条件）。

为什么说它好？因为：

• 没有奇点——随便你怎么转，不会像欧拉角那样卡住
• 计算快——只有4个参数，乘法比旋转矩阵少一半多
• 容易插值——做平滑旋转时，球面线性插值（SLERP）非常优雅

我记得第一次用四元数做姿态解算时，代码写出来自己都惊讶——那么复杂的旋转，几行就搞定了。而且更新频率从100Hz直接干到400Hz，CPU还凉凉的。

1.4 三种表示方法的转换

实际工程中，这三种表示方法经常需要互相转换。比如传感器输出欧拉角，但卡尔曼滤波用四元数，最后显示又要转回欧拉角。下面是我常用的转换代码：

// 四元数转欧拉角（ZYX顺序）
float roll  = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
float pitch = asin(2*(w*y - z*x));
float yaw   = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));

// 欧拉角转四元数
float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
float cr = cos(roll * 0.5);
float sr = 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;

这段代码我用了快十年，每次新项目直接复制粘贴。但要注意：欧拉角的旋转顺序很重要，不同行业习惯不同。航空航天常用ZYX，机器人可能用XYZ。一定要确认清楚。

知识体系总览

下面这张图是我自己画的，把本章的核心逻辑串起来了。你可以看到姿态估计的三种表示方法，以及它们之间的关系和适用场景。

从这张图可以清楚看到：欧拉图直观但有限制，旋转矩阵数学完美但笨重，四元数则是工程实践中的最优解。后面的课程里，我们会一直用四元数作为姿态状态量。