3. 刚体运动学建模：位置、速度、加速度与角速度的关系

各位同学，欢迎来到动力学建模的核心环节。今天我们要聊的，是无人机飞控算法里最基础、也最容易出错的一块——刚体运动学建模。

说白了，就是搞清楚无人机在三维空间里怎么动。位置怎么变？速度怎么算？角速度又是什么鬼？这些搞不清楚，后面PID调得再好，飞机也飞不稳。

我个人习惯，先把坐标系定下来。没有坐标系，一切免谈。

3.1 坐标系：世界坐标系与机体坐标系

无人机运动学建模，至少需要两套坐标系：

• 世界坐标系（惯性系）：固定在地面，通常取北-东-地（NED）或东-北-天（ENU）。我习惯用NED，因为和大多数开源飞控一致。
• 机体坐标系：固定在无人机上，原点在质心，x轴指向机头，y轴指向右翼，z轴指向下（右手定则）。

嗯，这里要注意：两个坐标系之间的转换，全靠旋转矩阵。你想想看，无人机在空中翻滚，机头指向变了，但地面的北方向没变。怎么把机体上的传感器读数换算到地面？靠的就是旋转矩阵。

3.2 位置、速度、加速度：线运动学

先讲线运动学。这个相对简单，就是牛顿第二定律在三维空间的推广。

设无人机在世界坐标系中的位置为 p = [x, y, z]^T，那么：

• 速度：v = dp/dt = [ẋ, ẏ, ż]^T
• 加速度：a = dv/dt = [ẍ, ÿ, z̈]^T

看起来很简单对吧？但我在项目中遇到过一个问题：加速度计测量的加速度，是机体坐标系下的。而我们要的是世界坐标系下的加速度，用来做位置控制。

所以，必须把加速度计读数通过旋转矩阵转换到世界坐标系。这个转换，我建议写成函数，别每次都手算，容易出错。

3.3 角速度与姿态：角运动学

角运动学比线运动学复杂一点。为什么？因为角速度不是姿态的简单导数。

无人机的姿态通常用欧拉角（滚转φ、俯仰θ、偏航ψ）或四元数表示。而陀螺仪直接测量的是机体角速度 ω = [p, q, r]^T。

那么，欧拉角的变化率和机体角速度之间是什么关系？

公式如下：

[φ̇]   [1  sinφ tanθ  cosφ tanθ] [p]
[θ̇] = [0  cosφ        -sinφ    ] [q]
[ψ̇]   [0  sinφ/cosθ  cosφ/cosθ] [r]

这个矩阵，我称之为欧拉角运动学矩阵。注意看，当俯仰角 θ 接近 ±90° 时，tanθ 和 1/cosθ 会趋于无穷大。这就是所谓的万向锁问题。

3.4 四元数运动学方程

四元数没有奇异性，是飞控算法的首选。四元数 q = [q0, q1, q2, q3]^T 的更新方程如下：

q̇ = 0.5 * q ⊗ ω_body

其中 ⊗ 表示四元数乘法，ω_body = [0, p, q, r]^T 是扩展后的角速度向量。

写成矩阵形式：

[q̇0]   [ 0  -p  -q  -r] [q0]
[q̇1] = [ p   0   r  -q] [q1]
[q̇2]   [ q  -r   0   p] [q2]
[q̇3]   [ r   q  -p   0] [q3]

这个方程，每次飞控循环都要跑一遍。我习惯用定点数或单精度浮点，计算量不大，但要注意归一化——四元数必须保持单位长度，否则姿态会漂移。

3.5 完整运动学方程汇总

把线运动和角运动合在一起，就是刚体运动学的完整模型：

物理量	方程	说明
位置	ṗ = v	世界坐标系
速度	v̇ = a_world	加速度需转换坐标系
姿态（四元数）	q̇ = 0.5 * q ⊗ ω_body	无奇异性
角速度	ω_body 由陀螺仪测量	机体坐标系

这个表格，我建议你贴在工位上。每次写代码前看一眼，确认自己用的是哪个坐标系。

3.6 知识体系结构图

下面这张图，是我自己梳理的刚体运动学知识结构。你可以把它当作思维导图来用。

这张图把整个运动学模型分成了四个模块：坐标系、线运动、角运动、关键方程。你写代码时，按这个结构来组织函数，逻辑会清晰很多。

3.7 实际项目中的注意事项

最后，分享几个我在实际项目中踩过的坑：

• 坐标系搞反：旋转矩阵的方向搞错，飞机往反方向飞。我建议每次写完旋转函数，先用单位向量测试一下。
• 四元数忘记归一化：姿态慢慢漂移，最后失控。归一化要放在每个控制循环里，别偷懒。
• 欧拉角微分方程用错：特别是俯仰角接近90度时，数值不稳定。我后来全部改用四元数，世界清净了。

好了，刚体运动学建模就讲到这里。记住，坐标系是基础，四元数是核心，归一化是关键。把这些搞明白，后面的动力学和控制学才能站得住脚。

---