3. 刚体运动学：位置、姿态与运动学方程

各位同学，大家好。今天我们进入四旋翼建模的核心环节——刚体运动学。说实话，这部分内容看起来全是公式，但它是整个飞控算法的地基。你想想看，如果连无人机怎么动、怎么转都描述不清楚，那后面的控制、辨识就全是空中楼阁。

我个人习惯把运动学拆成两大部分：位置运动学和姿态运动学。前者管“往哪飞”，后者管“怎么转”。两者通过一个关键关系——速度与角速度——串联起来，最终形成完整的运动学方程。

3.1 位置运动学：描述“飞到了哪里”

位置运动学，说白了就是回答一个问题：无人机在三维空间中的坐标是多少？

我们通常定义两个坐标系：

  • 地球固连坐标系（E系）：原点在地面某点，X轴指向北，Y轴指向东，Z轴指向地心（NED系）。这是我们的参考系。
  • 机体坐标系（B系）：原点在无人机质心，X轴指向机头，Y轴指向右翼，Z轴指向下方。

位置运动学描述的是：机体坐标系原点在地球坐标系中的位置变化。用数学语言说，就是位置向量 PE = [x, y, z]T 对时间的导数，等于机体速度在地球坐标系下的投影。

核心公式：

ṖE = VE

其中 VE 是无人机相对于地面的速度，在地球坐标系中表示。

嗯，这里要注意：我们实际控制时，传感器（比如GPS）直接给出的是 PE 和 VE。但飞控内部计算时，经常需要把 VE 转换到机体坐标系下，这就引出了姿态的作用。

3.2 姿态运动学：描述“转到了什么角度”

姿态运动学比位置运动学复杂一些。它描述的是：机体坐标系相对于地球坐标系的旋转状态。

常用的姿态表示方法有三种：

  • 欧拉角：滚转角 φ、俯仰角 θ、偏航角 ψ。直观，但有万向锁问题。
  • 旋转矩阵：3×3正交矩阵 REB。无奇点，但参数冗余。
  • 四元数：q = [q0, q1, q2, q3]T。无奇点，计算高效，是工程首选。

我个人在项目中强烈推荐使用四元数。为什么？因为欧拉角在俯仰角接近 ±90° 时，滚转和偏航会耦合，导致控制发散。我曾经在一次高机动飞行测试中，就因为用了欧拉角做姿态解算，结果飞机一个筋斗翻过去，姿态直接炸了……从那以后，我所有飞控代码都改用四元数。

3.3 速度与角速度的关系

这是运动学中最容易混淆的地方。很多初学者会问：角速度 ω 和线速度 V 之间到底有什么关系？

答案是：没有直接关系。它们是两个独立的物理量。

  • 线速度 V：描述质心位置的变化率，单位 m/s。
  • 角速度 ω：描述机体绕自身轴的旋转速率，单位 rad/s。

但两者通过姿态间接关联。举个例子：

当无人机以角速度 ω 旋转时，机体上任意一点（比如旋翼末端）的线速度，等于 ω × r（r 是该点相对于质心的位置向量）。这就是刚体运动学中著名的速度合成公式。

避坑指南：

我曾经在辨识旋翼气动参数时，忽略了角速度对旋翼末端速度的贡献，导致辨识结果偏差很大。后来才意识到，旋翼在旋转时，机体的角速度会叠加到旋翼的相对风速上。这个耦合效应在高速机动时不可忽略。

3.4 运动学方程推导

好了，现在我们把位置和姿态的运动学方程写在一起，就得到了完整的刚体运动学模型。

3.4.1 位置运动学方程

ṖE = REB · VB

其中：

  • ṖE：位置向量在地球系下的导数
  • REB：从机体系到地球系的旋转矩阵
  • VB：机体速度在机体系下的表示

3.4.2 姿态运动学方程（四元数形式）

q̇ = 0.5 · q ⊗ ωB

其中：

  • q̇：四元数对时间的导数
  • ⊗：四元数乘法
  • ωB：机体角速度，以纯四元数形式表示 [0, ωx, ωy, ωz]T

重要提醒：

四元数运动学方程必须保证 q 的模长始终为 1。数值积分过程中，由于误差累积，模长会漂移。我建议每步积分后都做一次归一化处理，否则姿态会慢慢“变形”。

3.4.3 完整运动学方程

将位置和姿态方程联立，得到：

状态向量：X = [x, y, z, q0, q1, q2, q3]T
输入向量：U = [Vx, Vy, Vz, ωx, ωy, ωz]T

运动学方程：
ẋ = REB(q) · VB
q̇ = 0.5 · q ⊗ ωB

这个方程组就是四旋翼运动学的基础。你想想看，有了它，我们就可以根据输入的线速度和角速度，推算出任意时刻无人机的位置和姿态。这就是飞控中“状态估计”的理论依据。

3.5 知识体系总览

为了让大家更直观地理解本章的知识结构，我画了一张图：

刚体运动学知识体系 刚体运动学 位置运动学 姿态运动学 地球坐标系 vs 机体坐标系 位置向量 PE = [x, y, z]T ṖE = REB · VB 欧拉角 / 旋转矩阵 / 四元数 角速度 ω = [p, q, r]T q̇ = 0.5 · q ⊗ ωB 速度与角速度关系：V = ω × r （刚体上任意点的线速度合成）

这张图把本章的核心逻辑串起来了。从刚体运动学出发，分支出位置和姿态两条线，最后通过速度与角速度的关系，把两者在物理意义上连接起来。你仔细看，位置运动学方程里用到了旋转矩阵 REB，而旋转矩阵正是由姿态四元数计算得到的。这就是耦合所在。

3.6 工程中的实用建议

最后，我分享几个实际项目中的经验：

  1. 坐标系定义要统一：我见过太多团队因为坐标系定义不一致，导致算法移植时出问题。建议在代码开头用宏定义明确坐标系方向。
  2. 四元数初始化要小心：上电时，如果姿态初始值不准，后续积分会一直偏。我习惯用加速度计和磁力计做一次初始对准，再切换到四元数积分。
  3. 数值积分方法选择：对于运动学方程，我推荐使用四阶龙格-库塔法（RK4）。欧拉法虽然简单，但精度不够，长时间积分会累积明显误差。
  4. 注意奇异值：虽然四元数没有万向锁，但旋转矩阵在某些姿态下会接近奇异。做矩阵求逆时，记得加一个小的正则化项。

本章小结：

刚体运动学是四旋翼建模的“骨架”。位置运动学告诉我们无人机在哪，姿态运动学告诉我们无人机朝向哪，而速度与角速度的关系则揭示了刚体上各点的运动规律。掌握了这些，你就有了描述无人机运动状态的全部工具。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →