第2章无人机运动学与动力学基础

2、无人机运动学与动力学基础：坐标系定义与模型简化

各位同学，欢迎来到第二章。这一章的内容，说白了就是给无人机建立一套「数学语言」。你想想看，要让飞机听话，首先得知道它在哪、怎么转、怎么飞。坐标系、姿态表示、动力学模型——这些就是最底层的砖瓦。

我个人习惯，每次接手一个新飞控项目，第一件事就是先把坐标系理清楚。为什么？因为坐标系搞混了，后面所有算法都是空中楼阁。我在项目中就遇到过，因为导航系和机体系的转换搞反了，飞机直接朝反方向飞——嗯，那次炸鸡的教训挺深刻的。

2.1 坐标系定义

无人机里常用的坐标系有三个：世界系、机体系、导航系。它们各有各的用处，千万别混用。

2.1.1 世界坐标系（W系）

世界系是固定在地球上的。通常取北、东、地（NED）或者北、东、天（ENU）。我建议初学者用NED，因为大多数开源飞控（比如PX4）都用这个。原点可以选在起飞点，X轴指向北，Y轴指向东，Z轴指向地心。

  重要： 世界系是惯性参考系，所有绝对位置、速度、加速度都在这个系下描述。

2.1.2 机体坐标系（B系）

机体系是固定在飞机上的。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。

为什么要搞个机体系？因为传感器（IMU、磁力计）都是装在飞机上的，它们测到的数据天然就是机体系下的。比如加速度计测的是「飞机本身的加速度」，而不是「飞机相对于地面的加速度」。

我的经验： 机体系和世界系之间的转换，是飞控里最频繁的计算。我建议把转换矩阵提前算好，不要每次都重新算，能省不少CPU。

2.1.3 导航坐标系（N系）

导航系其实可以理解为「局部世界系」。它和世界系一样是固定的，但原点可以移动。比如你让无人机绕着一个点飞行，那个点就是导航系的原点。导航系通常也取NED方向。

说白了，导航系就是「以任务为中心」的坐标系。我在做编队飞行时，经常把长机的位置设为导航系原点，僚机的位置都相对于这个原点来描述——这样控制起来特别直观。

2.2 欧拉角与四元数

飞机的姿态怎么表示？最直观的是欧拉角，最实用的是四元数。这两个东西，你必须都得会。

2.2.1 欧拉角

欧拉角就是三个角度：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。旋转顺序我建议用ZYX（先偏航、再俯仰、最后横滚），这是航空界的惯例。

欧拉角的优点是直观。你一看「横滚30度」，就知道飞机向右倾斜了30度。但缺点也很致命——万向锁。当俯仰角接近±90度时，横滚和偏航会耦合，导致姿态解算失效。

避坑指南： 我曾经在某个项目中，直接用欧拉角做姿态控制，结果飞机做大机动时直接失控。后来换成四元数才解决。所以，千万不要在控制回路里直接用欧拉角，只适合给人看。

2.2.2 四元数

四元数是一个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它没有万向锁问题，而且插值平滑，是飞控里的首选。

四元数和欧拉角可以互相转换。公式如下：

// 欧拉角转四元数（ZYX顺序）
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);

q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;

我的习惯： 在飞控里，所有姿态运算都用四元数。只在日志记录和地面站显示时，才转成欧拉角。这样既避免了万向锁，又方便调试。

2.3 无人机动力学模型简化

真实的无人机动力学非常复杂，有空气动力学、电机动力学、结构弹性等等。但在飞控算法里，我们通常做简化。你想想看，控制周期只有几毫秒，哪有时间算那么细？

我常用的简化模型是这样的：

刚体假设： 认为飞机是刚体，不考虑形变
低速飞行： 忽略空气阻力（或者用简单阻尼代替）
小角度假设： 横滚和俯仰角较小，sinθ≈θ，cosθ≈1

简化后的动力学方程：

// 位置动力学（世界系）
ddot_x = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * T / m
ddot_y = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * T / m
ddot_z = cos(phi)*cos(theta) * T / m - g

// 姿态动力学（机体系）
p_dot = (Iyy - Izz) / Ixx * q * r + tau_x / Ixx
q_dot = (Izz - Ixx) / Iyy * p * r + tau_y / Iyy
r_dot = (Ixx - Iyy) / Izz * p * q + tau_z / Izz

其中 T 是总推力，tau 是力矩，I 是转动惯量。这个模型虽然简单，但足够覆盖大部分飞行场景。

  注意： 小角度假设只在悬停和低速飞行时成立。如果你要做特技飞行，必须用完整的非线性模型。

2.4 控制分配矩阵

控制分配矩阵，说白了就是把「期望的力和力矩」转换成「每个电机的转速」。这是飞控里最后一步，也是最容易出错的一步。

对于四旋翼，控制分配矩阵是4x4的：

// 控制分配矩阵（X型四旋翼）
// [T, tau_x, tau_y, tau_z]^T = M * [w1^2, w2^2, w3^2, w4^2]^T

M = [ 1,    1,    1,    1;
      -l,   -l,    l,    l;
      -l,    l,    l,   -l;
      -c,    c,   -c,    c ];

其中 l 是臂长，c 是反扭矩系数。注意，不同机型（X型、+型）的矩阵不一样。我在项目中就见过有人把X型和+型的矩阵搞混，结果飞机起飞就翻——嗯，那个画面挺尴尬的。

我的建议： 控制分配矩阵最好写成参数化的形式，方便不同机型切换。我一般会在配置文件中定义机架类型，然后自动生成对应的矩阵。

2.5 知识体系总览

下面这张图，是我自己画的本章知识体系。你可以把它当作一个「导航图」，学完本章后，你应该能把这几个模块串起来。

嗯，这一章的内容就到这里。坐标系、姿态表示、动力学模型、控制分配——这四个东西是飞控的「四根柱子」。你把这四根柱子立稳了，后面的避障算法才能盖得上去。

  课后作业： 自己推导一下四旋翼的控制分配矩阵，看看如果电机顺序变了，矩阵该怎么改。这个练习能帮你真正理解分配矩阵的物理意义。

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

📚 课程目录

2、无人机运动学与动力学基础：坐标系定义与模型简化

2.1 坐标系定义

2.1.1 世界坐标系（W系）

2.1.2 机体坐标系（B系）

2.1.3 导航坐标系（N系）

2.2 欧拉角与四元数

2.2.1 欧拉角

2.2.2 四元数

2.3 无人机动力学模型简化

2.4 控制分配矩阵

2.5 知识体系总览