姿态解算基础-坐标系定义、旋转矩阵、加速度解算欧拉角

是 否 +2 论坛币

k人 参与回答

经管之家送您一份

应届毕业生专属福利!

求职就业群

赵安豆老师微信：zhaoandou666

经管之家联合CDA

送您一个全额奖学金名额~ !

立即领取

感谢您参与论坛问题回答

经管之家送您两个论坛币！

+2 论坛币

在无人机导航与姿态描述中，坐标系的定义和旋转顺序是理解飞行器运动状态的基础。以下是对相关概念的系统性阐述。

1. 坐标系定义

导航坐标系（Navigation Frame, n-frame）
该坐标系通常作为无人机运动的参考基准，原点 $ O_n $ 设定在起飞点或当前位置于地球表面的投影位置。它属于局部切平面坐标系（Local Tangent Plane），在短距离飞行任务中可近似视为惯性系（忽略地球自转带来的科里奥利效应）。

采用北东地（NED, North-East-Down）约定：

• $ X_n $ 轴：指向地理正北方向；
• $ Y_n $ 轴：指向地理正东方向；
• $ Z_n $ 轴：垂直地表向下，指向地心，与重力方向一致。

该坐标系为后续姿态变换提供静态参考框架。

atan(y/z)

机体坐标系（Body Frame, b-frame）
此坐标系固定于无人机本体上，随其一起移动和旋转，原点 $ O_b $ 一般设在飞行器的重心（CoG）处。

采用前右下（FRD, Front-Right-Down）约定：

• $ X_b $ 轴：沿机头方向向前延伸；
• $ Y_b $ 轴：指向机身右侧；
• $ Z_b $ 轴：垂直于机体平面向下（即朝向机腹方向）。

惯性测量单元（IMU）采集的加速度、角速度等原始数据均在此坐标系下表达。

atan

2. 欧拉角表示法（Euler Angles）

欧拉角用于描述机体坐标系 $ b $ 相对于导航坐标系 $ n $ 的旋转姿态，遵循航空领域的 Tait-Bryan 角惯例，包含三个基本角度：

滚转角（Roll, $ \phi $）
绕 $ X_b $ 轴的旋转量。
正方向依据右手定则确定：当右翼下沉、左翼抬升时为正值，表示向右滚转。
取值范围：$ (-\pi, \pi] $。

俯仰角（Pitch, $ \theta $）
绕 $ Y_b $ 轴的旋转量。
正方向为机头抬起（抬头动作）。
取值范围：$ [-\pi/2, \pi/2] $。
注意：该角存在奇点问题，可能导致万向节死锁现象。

偏航角（Yaw, $ \psi $）
绕 $ Z_b $ 轴的旋转量。
正方向为机头向右偏转（从上方观察呈顺时针方向）。
取值范围：$ (-\pi, \pi] $。

这三个角度共同构建了从导航系到机体系的姿态映射关系。

atan2

3. 旋转顺序说明

在飞控系统（如PX4、ArduPilot）中，标准的旋转顺序为 Z-Y-X，也称为“3-2-1”顺序。这是一种内旋（Intrinsic Rotation）方式，意味着每次旋转都基于前一次旋转后形成的新坐标轴进行。

具体变换过程如下（从导航系 $ n $ 到机体系 $ b $）：

1. 首先绕 $ Z_n $ 轴旋转偏航角 $ \psi $，得到第一个中间坐标系；
2. 然后绕新生成的 $ Y_1 $ 轴旋转俯仰角 $ \theta $，进入第二个中间状态；
3. 最后绕更新后的 $ X_2 $ 轴旋转滚转角 $ \phi $，使坐标系最终与机体坐标系 $ b $ 完全重合。

这种动轴旋转机制决定了方向余弦矩阵的乘积顺序为 $ R = R_z(\psi) R_y(\theta) R_x(\phi) $。

atan2(y, x)

4. 基本旋转矩阵（Direction Cosine Matrix, DCM）

根据右手定则（大拇指指向轴正向，四指弯曲方向为正旋转方向），定义以下基本旋转矩阵。这些矩阵用于实现向量在不同坐标系之间的投影转换，属于坐标系旋转、向量不动的情形。

绕 X 轴旋转 $ \phi $：
$$ R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{bmatrix} $$

绕 Y 轴旋转 $ \theta $：
$$ R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{bmatrix} $$

绕 Z 轴旋转 $ \psi $：
$$ R_z(\psi) = \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} $$

上述三个矩阵分别对应单一轴的坐标变换操作，组合后可构成完整的从导航系到机体系的方向余弦矩阵。[此处为图片5]

在姿态解算中，旋转操作通常通过方向余弦矩阵（Direction Cosine Matrix, DCM）或旋转矩阵来描述。这两者本质上表示相同的内容，但在不同语境下强调的侧重点有所区别。

方向余弦矩阵（DCM） 从几何角度出发，定义为两个坐标系之间各轴夹角的余弦值所构成的矩阵。例如，设有参考坐标系 \( O-XYZ \)（如导航系）和机体坐标系 \( O-x'y'z' \)（随飞行器运动），则 DCM 的每个元素代表一个坐标轴在另一坐标系中的投影。

以绕 Z 轴旋转为例，基本旋转矩阵可表示为：

\[ R_z(\psi) = \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

atan(y/z)

该矩阵用于实现从导航系到机体系的坐标变换。值得注意的是，还存在另一种形式——主动旋转（active rotation）下的旋转矩阵，其结构为上述矩阵的转置，适用于向量本身的旋转而非坐标系变换：

\[ R_z(\psi)_{\text{active}} = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

atan

这种差异源于变换类型的不同：前者为被动变换（坐标系旋转，点不变），后者为主动变换（点旋转，坐标系不变）。

当考虑三维空间中的完整姿态时，需将绕三个轴的旋转组合起来。通常采用欧拉角顺序进行内旋（即动轴旋转），例如先偏航（Yaw, \(\psi\)）、再俯仰（Pitch, \(\theta\)）、最后滚转（Roll, \(\phi\)）。由于是内旋，总旋转矩阵按从左到右的顺序相乘：

\[ C_n^b = R_x(\phi) \cdot R_y(\theta) \cdot R_z(\psi) \]

最终得到从导航系 (\(n\)) 到机体系 (\(b\)) 的完整旋转矩阵 \( C_n^b \)，也常记作 \( R_n^b \)。其具体表达式如下：

\[ C_n^b = \begin{bmatrix} \cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta \\ \sin\phi\sin\theta\cos\psi - \cos\phi\sin\psi & \sin\phi\sin\theta\sin\psi + \cos\phi\cos\psi & \sin\phi\cos\theta \\ \cos\phi\sin\theta\cos\psi + \sin\phi\sin\psi & \cos\phi\sin\theta\sin\psi - \sin\phi\cos\psi & \cos\phi\cos\theta \end{bmatrix} \]

atan2

此矩阵是姿态解算中的核心工具之一，广泛应用于惯性导航、机器人学与计算机图形学等领域。

从物理意义上讲，该矩阵的每一列分别表示导航系中的单位向量在机体系下的分量。例如，重力向量在导航系中为 \([0,\, 0,\, g]^T\)，通过该矩阵变换后，即可获得其在机体坐标系中的三个轴向分量，便于传感器数据处理与姿态反馈控制。

综上所述，“方向余弦矩阵”与“旋转矩阵”在大多数实际应用中可视为等价概念。它们的区别主要体现在命名来源与理解视角：

• DCM 强调其几何构建方式——基于两组坐标轴之间的方向余弦；
• 旋转矩阵 更侧重代数功能——作为线性变换算子对向量执行旋转操作。

尽管表述角度不同，二者在数学形式和应用场景上高度统一，共同构成了多体系统姿态描述的基础框架。

为了描述机体坐标系相对于参考坐标系的姿态，关键在于明确机体的三个主轴（x′, y′, z′）在参考坐标系中的指向。这一姿态可以通过数学工具精确表达。

方向余弦的概念
在向量代数中，两个向量的点积等于它们模长的乘积与夹角余弦的乘积。若两向量均为单位向量（长度为1），则其点积直接等于二者夹角的余弦值。这一性质构成了“方向余弦”的基础。

方向余弦矩阵（DCM）的结构
方向余弦矩阵是一个3×3的矩阵，记作 C，其每个元素 C_ij 表示机体某一轴与参考系某一轴之间夹角的余弦：

C = \[ \begin{bmatrix} \cos(x', X) & \cos(x', Y) & \cos(x', Z) \\ \cos(y', X) & \cos(y', Y) & \cos(y', Z) \\ \cos(z', X) & \cos(z', Y) & \cos(z', Z) \end{bmatrix} \]

• 第一行：表示机体 x′ 轴在参考系 X, Y, Z 方向上的投影分量。
• 第二行：表示机体 y′ 轴在参考系各轴上的投影。
• 第三行：表示机体 z′ 轴在参考系中的投影。

本质上，DCM 的每一行（或列，依定义习惯而定）代表将机体坐标系的一个基向量投影到参考坐标系中的结果。

atan(y/z)

旋转矩阵的代数视角
旋转矩阵通常用符号 R 表示，更侧重于其作为线性变换的运算功能。当需要将一个向量从一个坐标系变换至另一个坐标系时，可通过矩阵乘法实现：

v_new = R v_old

该矩阵具备关键数学特性——正交性：
R^T = R¹，且行列式为 +1。这意味着旋转不改变向量长度，保持空间几何结构不变，仅调整方向。同时，逆变换可通过转置矩阵轻松获得。

为何存在两种名称？
“方向余弦矩阵”与“旋转矩阵”实指同一数学对象，命名差异源于不同领域的传统偏好：

• 航空航天与惯性导航领域：倾向使用“DCM”。在捷联惯导系统中，通过陀螺仪积分计算姿态时，直接利用轴间夹角的余弦关系具有清晰的物理意义。
• 计算机视觉、机器人学及3D图形学：更常用“旋转矩阵”。这些领域注重坐标变换的代数操作、SO(3)群的性质，以及与其他变换（如平移、缩放）的组合应用。

直观理解：投影视角
一个有效的记忆方式是：旋转矩阵的列向量对应新坐标系各轴在原坐标系中的方向。

设 R_nb 是将向量从机体系 b 变换到参考系 n 的旋转矩阵，即：

v_n = R_nb v_b

则该矩阵可表示为：

R_nb = \[ \left[ \begin{array}{c|c|c} & & \\ \vec{x}_b^n & \vec{y}_b^n & \vec{z}_b^n \\ & & \end{array} \right] \]

• 第一列：机体X轴（单位向量）在参考系中的坐标表示。
• 第二列：机体Y轴在参考系中的方向分量。
• 第三列：机体Z轴在参考系中的投影。

由此可见，方向余弦矩阵的本质正是将三个坐标轴的方向信息（即方向余弦）以列的形式组合成一个矩阵。

atan(y/z)

总结
方向余弦矩阵（DCM）与旋转矩阵是同一概念的两种表述：
- “DCM”强调其构造方式——由各轴间夹角的余弦构成；
- “旋转矩阵”突出其功能用途——用于向量旋转或坐标系转换。

在描述三维空间中的旋转时，尽管某些表示方法包含9个元素，但由于存在6个冗余约束（实际仅有3个自由度），其数值更新效率低于四元数（Quaternion）。然而，在直观理解当前姿态方向方面，这类方法仍具有明显优势。

当飞行器处于静止状态时，加速度计所测得的数据可用于计算姿态角。在前右下坐标系中，重力加速度在z轴方向的测量值通常为负，但此处采用正值g进行推导（使用负值结果一致），满足以下关系：

\[ \begin{bmatrix}a_x \\a_y \\a_z \end{bmatrix}=C_n^b\times\begin{bmatrix}0 \\0 \\g \end{bmatrix} \]

atan(y/z)

其中，方向余弦矩阵 \( C_n^b \) 将导航系下的重力矢量转换至机体坐标系，得到各轴加速度分量：

\[ \begin{cases} a_x = (-\sin\theta) \cdot g \\ a_y = (\sin\phi\cos\theta) \cdot g \\ a_z = (\cos\phi\cos\theta) \cdot g \end{cases} \]

接下来通过上述分量求解俯仰角（Pitch, \( \theta \)）：

由 \( a_x = -g \cdot \sin\theta \)，可得：

\[ -a_x = g \cdot \sin\theta \]

同时考虑横向与竖向分量的合成效果：

\[ \begin{cases} a_y = g \cdot \sin\phi \cdot \cos\theta \\ a_z = g \cdot \cos\phi \cdot \cos\theta \end{cases} \]

将两式平方相加：

\[ \begin{aligned} a_y^2 + a_z^2 &= (g \sin\phi \cos\theta)^2 + (g \cos\phi \cos\theta)^2 \\ &= g^2 \sin^2\phi \cos^2\theta + g^2 \cos^2\phi \cos^2\theta \\ &= g^2 \cos^2\theta (\sin^2\phi + \cos^2\phi) \\ &= g^2 \cos^2\theta \end{aligned} \]

对两边开根号，并假设俯仰角范围在 \(-90^\circ\) 到 \(90^\circ\) 之间，此时 \( \cos\theta > 0 \)，有：

\[ \sqrt{a_y^2 + a_z^2} = g \cdot \cos\theta \]

结合分子和分母表达式：

\[ \frac{-a_x}{\sqrt{a_y^2 + a_z^2}} = \frac{g \cdot \sin\theta}{g \cdot \cos\theta} = \tan\theta \]

因此，俯仰角可通过反正切函数求得：

\[ \theta = \operatorname{atan2}\left(-a_x, \sqrt{a_y^2 + a_z^2}\right) \]

由此可知，该公式计算出的俯仰角有效范围为 \([-\pi/2, \pi/2]\)。特别地，当z轴测得的加速度为 \(-g\) 时，同样适用此公式：

\[ \theta = \operatorname{atan2}(-a_x, \sqrt{a_y^2 + a_z^2}) \]

在姿态解算过程中，俯仰角（Pitch, θ）的计算公式如下：

θ = atan2(分子, 分母) = atan2(a_x, √(a_y + a_z))

该表达式通过将加速度计的三个轴分量进行三角函数处理，得到设备在空间中的倾斜角度。其中，分子为 x 轴的加速度分量 a_x，分母为 y 轴与 z 轴合加速度的模长。

atan(y/z)

接下来求解滚转角（Roll, φ）。根据加速度计模型，有以下关系：

a_y = g·sinφ·cosθ

a_z = g·cosφ·cosθ

atan

将两个等式相除，可得：

a_y/a_z = (g·sinφ·cosθ) / (g·cosφ·cosθ) = sinφ / cosφ = tanφ

因此可以推导出：

tanφ = a_y/a_z

进而得到：

φ = arctan(a_y/a_z)

atan2

然而，在实际工程实现中（如 C 语言或 Python 编程），应避免直接使用 arctan 函数，原因有两个：其一，当 a_z = 0 时，分母为零会导致程序异常；其二，arctan 无法准确判断象限，难以区分第一象限和第三象限的角度。

为此，必须采用 atan2 函数来替代：

φ = atan2(a_y, a_z)

当传感器测量到的加速度方向为 -g 时，理论上应考虑符号变化，即：

φ = atan2(-a_y, -a_z)

尽管该形式在代数推导中可能无法完全消去负号，但通过实际角度分析可知，若去除负号后所得结果与正 g 情况下的公式一致，则仍可采用标准形式进行计算。

atan2(y, x)

最终，在飞控代码中若使用了 atan2 函数，推荐的姿态解算公式如下：

Roll（滚转角）： φ = atan2(a_y, a_z)

Pitch（俯仰角）： θ = atan2(-a_x, √(a_y + a_z))

扫码加我 拉你入群

请注明：姓名-公司-职位

以便审核进群资格，未注明则拒绝

分享0 收藏0 回帖

关键词：旋转矩阵 坐标系 加速度 navigation Direction