2. 坐标系与位姿表示：世界坐标系、机体坐标系、欧拉角、四元数、旋转矩阵

各位同学，欢迎来到第二章。

说实话，搞无人机编队，第一道坎儿往往不是控制算法，而是「坐标系」和「位姿」这两个概念。我见过太多新手，代码写得飞起，结果飞机一上天就乱窜——最后发现是坐标系搞反了。嗯，我自己也踩过这个坑。

2.1 世界坐标系与机体坐标系

先聊最基础的。世界坐标系，你可以理解为「上帝视角」。它固定在大地上，通常用 W 表示。我们说的经纬高、北东地（NED），都属于世界坐标系。编队飞行里，每架飞机的目标位置，都是在这个坐标系下定义的。

机体坐标系，则是「飞机自己的视角」。它跟着飞机一起动，原点在飞机重心，X 轴指向机头，Y 轴指向右翼，Z 轴指向下（符合右手定则）。

为什么要区分这两个？因为传感器数据通常是在机体坐标系下采集的，而控制指令却要在世界坐标系下执行。举个例子：

关键点： 机载 GPS 给出的是世界坐标，但 IMU（惯性测量单元）测出的加速度是机体坐标。你必须把它们统一起来，否则控制律会算错方向。

我在项目中遇到过一件事：一架飞机要向右平移，结果它斜着飞出去了。查了半天，发现是姿态解算时忘了把机体坐标系下的加速度转换到世界坐标系。说白了，就是坐标系没对齐。

2.2 欧拉角：直观但小心「万向锁」

欧拉角是最直观的位姿表示法。它用三个角度描述飞机的姿态：滚转角（Roll，φ）、俯仰角（Pitch，θ）、偏航角（Yaw，ψ）。

你可以这样记：

  • 滚转：绕机体 X 轴旋转，像飞机侧身转弯。
  • 俯仰：绕机体 Y 轴旋转，像飞机抬头低头。
  • 偏航：绕机体 Z 轴旋转，像飞机原地转向。

欧拉角的旋转顺序很重要。我习惯用 Z-Y-X 顺序（先偏航，再俯仰，最后滚转）。为什么？因为这种顺序在无人机里最常用，而且物理意义清晰。

避坑指南： 我曾经在代码里直接用欧拉角做插值，结果飞机在俯仰接近 90° 时突然失控。这就是著名的「万向锁」问题——当俯仰角为 ±90° 时，滚转和偏航会失去一个自由度。所以，如果你要做连续旋转或插值，千万别用欧拉角。

欧拉角的数学表达很简单，但我不建议你在控制循环里直接用。原因有二：一是万向锁，二是计算效率低。那用什么？往下看。

2.3 四元数：优雅且无奇点

四元数，听起来高大上，其实就是一个四维向量：q = [w, x, y, z]。其中 w 是实部，x、y、z 是虚部。它没有奇点，计算效率高，是无人机姿态解算的标配。

你可能会问：「为什么不用欧拉角？」

原因很简单：四元数可以平滑地表示任意旋转，而且插值非常自然。我在做编队队形变换时，所有飞机的姿态都是用四元数来传递的。因为用欧拉角做插值，飞机姿态会「跳变」，而四元数插值（Slerp）则丝滑得像德芙巧克力。

来看一个 ROS 里的例子：

# Python 示例：使用 scipy 进行四元数旋转
from scipy.spatial.transform import Rotation as R
import numpy as np

# 定义一个四元数 [w, x, y, z]
q = [0.707, 0.707, 0, 0]  # 绕 X 轴旋转 90°
r = R.from_quat(q)

# 将一个向量从机体坐标系转到世界坐标系
v_body = np.array([1, 0, 0])  # 机头方向
v_world = r.apply(v_body)
print(f"世界坐标系下的向量: {v_world}")

嗯，这里要注意：ROS 里四元数的顺序是 [x, y, z, w]，而 scipy 默认是 [x, y, z, w]，但有些库是 [w, x, y, z]。我建议你统一用 ROS 的规范，否则容易出 bug。

个人经验： 在编队控制中，我习惯把四元数归一化后再使用。因为非归一化的四元数会导致旋转矩阵不正交，进而影响控制精度。每次收到 IMU 数据，第一件事就是归一化。

2.4 旋转矩阵：坐标变换的「硬核」工具

旋转矩阵是一个 3x3 的正交矩阵，它可以把一个坐标系下的向量变换到另一个坐标系。说白了，它就是坐标变换的「翻译官」。

从四元数到旋转矩阵的转换公式如下：

# 从四元数到旋转矩阵
def quaternion_to_rotation_matrix(q):
    # q = [w, x, y, z]
    w, x, y, z = q
    R = np.array([
        [1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
        [2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
        [2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
    ])
    return R

你想想看，有了旋转矩阵，你就可以把机体坐标系下的速度、加速度、力，统统转换到世界坐标系。编队控制里，每架飞机的相对位置计算，都离不开它。

举个例子：编队中长机在 (0, 0, 0)，僚机在 (10, 0, 0)。如果长机偏航了 30°，僚机的目标位置也要跟着旋转。这时候，旋转矩阵就派上用场了。

核心总结：

  • 欧拉角：直观，但有万向锁，适合人机交互。
  • 四元数：无奇点，适合计算和插值，是 ROS 的标准。
  • 旋转矩阵：适合坐标变换，但计算量大，通常由四元数生成。

2.5 实战：在 ROS 中处理位姿

最后，我分享一个 ROS 里的实用技巧。在编队控制中，我们经常需要获取飞机的当前位姿。ROS 的 /mavros/local_position/pose 话题会发布 PoseStamped 消息，里面包含位置（xyz）和姿态（四元数）。

# 从 ROS 话题中提取位姿
import rospy
from geometry_msgs.msg import PoseStamped
from scipy.spatial.transform import Rotation as R

def pose_callback(msg):
    # 位置
    pos = msg.pose.position
    # 四元数 (ROS 顺序: x, y, z, w)
    quat = [msg.pose.orientation.x,
            msg.pose.orientation.y,
            msg.pose.orientation.z,
            msg.pose.orientation.w]
    
    # 转成欧拉角（方便调试）
    r = R.from_quat(quat)
    euler = r.as_euler('zyx', degrees=True)
    print(f"位置: ({pos.x:.2f}, {pos.y:.2f}, {pos.z:.2f})")
    print(f"姿态: (Yaw={euler[0]:.1f}°, Pitch={euler[1]:.1f}°, Roll={euler[2]:.1f}°)")

rospy.init_node('pose_listener')
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback)
rospy.spin()

嗯，这里要提醒你：千万不要在控制循环里频繁做四元数到欧拉角的转换。因为 as_euler() 涉及反三角函数，计算开销大。我一般只在调试时打印欧拉角，控制逻辑里全程用四元数。

我曾经犯过的错： 在编队控制中，我直接用欧拉角做差值计算，结果飞机在俯仰角接近 90° 时，控制量突然跳变，差点炸机。从那以后，我所有姿态相关的计算都改用四元数。记住：欧拉角只适合给人看，不适合给机器算。

好了，这一章的内容就到这里。下一章我们会聊「编队通信与数据同步」，到时候你会看到这些坐标系和位姿知识是如何在真实系统中落地的。

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