第2章 坐标系与运动学模型

各位同学，欢迎来到第二章。这一章的内容，说白了就是给无人机编队「定规矩」——你得知道飞机在哪儿、朝哪儿飞、怎么描述它的运动。我当年刚入行时，觉得坐标系这东西太基础了，结果第一次做编队仿真，坐标转换搞反了，两架飞机在仿真里直接「穿模」……嗯，从那以后我再也不敢小看坐标系了。

2.1 地球坐标系：我们脚下的参考系

地球坐标系，也叫大地坐标系。它固定在地球上，用来描述无人机相对于地面的位置。常用的有两种：

• 地理坐标系（LLA）：经度、纬度、高度。GPS直接输出的就是这个。
• 地心地固坐标系（ECEF）：以地心为原点，X轴指向本初子午线与赤道交点，Y轴指向东经90度，Z轴指向北极。

我个人习惯在航迹规划时，先把GPS数据转成ECEF，再转成局部东北天坐标系（ENU）。为什么？因为ENU坐标系下计算距离和角度更直观，你想想看，编队控制里我们经常说「你往东飞10米，往北飞5米」，这不就是ENU吗？

2.2 机体坐标系：飞机自己的视角

机体坐标系固定在无人机上。原点在质心，X轴指向机头，Y轴指向右翼，Z轴指向下方（符合右手定则）。

为什么要用机体坐标系？因为传感器（IMU、磁力计）测量的数据都是相对于机体的。比如加速度计告诉你「我在X方向有0.5g的加速度」，这个X方向就是机体坐标系的X轴。

我在项目中遇到过一个问题：某次试飞，无人机偏航角解算一直有误差。查了半天，发现是机体坐标系定义反了——Z轴朝上了。嗯，这种低级错误，犯一次就记住了。

2.3 坐标转换矩阵：连接两个世界的桥梁

从机体坐标系到地球坐标系，或者反过来，都需要坐标转换矩阵。最常用的是基于欧拉角的旋转矩阵。

假设无人机当前的姿态为：滚转角φ、俯仰角θ、偏航角ψ。那么从机体坐标系到地球坐标系的旋转矩阵为：

R = Rz(ψ) * Ry(θ) * Rx(φ)

其中：
Rz(ψ) = [cosψ  -sinψ  0;  sinψ  cosψ  0;  0  0  1]
Ry(θ) = [cosθ   0   sinθ;  0     1    0; -sinθ  0  cosθ]
Rx(φ) = [1    0      0;  0  cosφ  -sinφ;  0  sinφ  cosφ]

注意旋转顺序：先滚转，再俯仰，最后偏航。这个顺序不能乱，否则结果完全不对。我曾经在代码里把顺序写反了，仿真出来的无人机像喝醉了酒一样乱转……

2.4 无人机质点运动学模型

在航迹规划层面，我们通常把无人机简化为一个质点。为什么？因为规划的是路径，不是姿态控制。姿态控制是底层飞控的事。

质点运动学模型如下：

状态量：x = [px, py, pz, v, ψ]^T
其中：
  px, py, pz：位置（地球坐标系）
  v：速度大小（地速）
  ψ：偏航角

控制量：u = [a, ω]^T
其中：
  a：加速度（沿速度方向）
  ω：偏航角速率

运动方程：
  d(px)/dt = v * cos(ψ)
  d(py)/dt = v * sin(ψ)
  d(pz)/dt = vz（垂直速度，通常单独控制）
  dv/dt = a
  dψ/dt = ω

这个模型很简单，但够用。我在做城市环境编队飞行时，用的就是它。你想想看，规划层只需要告诉飞控「下一时刻你该往哪飞、飞多快」，具体的姿态控制交给底层。

2.5 编队相对运动学模型

编队控制的核心，不是每架无人机飞到哪里，而是它们之间的相对位置关系。说白了，就是「你在我左边10米，他在我后面5米」这种关系。

假设编队中有N架无人机，我们选一架作为长机（Leader），其余为僚机（Follower）。僚机的目标位置相对于长机是固定的：

僚机i的目标位置（地球坐标系）：
  p_i_target = p_leader + R(ψ_leader) * d_i

其中：
  p_leader：长机位置
  R(ψ_leader)：长机偏航角对应的旋转矩阵
  d_i：僚机i在长机体坐标系下的期望偏移量

这个公式很关键。它把编队保持问题，转化成了每个僚机跟踪一个动态目标点的问题。我在实际项目中，用这个模型实现了5架无人机的菱形编队，效果不错。

但要注意：当长机做大机动转弯时，僚机的目标点会快速移动。如果控制频率不够高，僚机可能跟不上。我曾经遇到过这种情况，编队直接散架了……后来把控制频率从10Hz提到了50Hz，问题解决。

2.6 本章知识体系

下面这张图，是我画的本章知识结构。你可以看到，从地球坐标系出发，经过坐标转换，到机体坐标系，再到运动学模型，最后到编队相对运动学，是一条清晰的链路。

2.7 本章小结

这一章的内容，是后续所有航迹规划算法的基础。你想想看，如果连坐标系都没搞清楚，规划出来的路径怎么可能正确？

我个人建议，初学者先把坐标转换矩阵的推导手算一遍，再用代码验证。这个过程虽然枯燥，但能帮你建立直觉。我在带新人时，都会让他们先过这一关。

好了，坐标系和运动学模型就讲到这里。下一章我们会进入真正的航迹规划算法——A*算法和它的变种。到时候你会发现，坐标系的知识会反复用到。