第二章 坐标系与运动学基础
各位同学，今天我们来聊聊坐标系和运动学。说实话，这部分内容看起来有点枯燥，但它是整个多目标航迹规划的基石。你想想看，连飞机在哪儿、往哪儿飞都说不清楚，还谈什么路径规划？
我个人习惯把坐标系比作「语言的语法」。没有统一的语法，你说你的、我说我的，信息就乱套了。在无人机领域，我们至少得掌握三种「语法」：地心地固、北东地、还有机体坐标系。
2.1 常用坐标系
2.1.1 地心地固坐标系（ECEF）
这个坐标系，说白了就是把地球当成一个椭球体。原点在地心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
我在项目中遇到过一个问题：用GPS数据直接做局部路径规划，结果飞机在极地附近飘得离谱。为什么？因为GPS给的是经纬高，而ECEF才是真正的笛卡尔坐标系。你拿经纬度直接算距离，误差会随着纬度升高而放大。
关键点：ECEF是全局坐标系，适合做长距离航迹规划。但它的数值变化范围大（几千公里量级），做局部规划时精度不够。
2.1.2 北东地坐标系（NED）
NED坐标系，原点通常选在无人机起飞点或地面站位置。X轴指向北，Y轴指向东，Z轴指向地心（也就是「下」）。
嗯，这里要注意：NED是右手坐标系，但Z轴向下。很多新手会搞混，以为Z轴向上。我曾经因为这个错误，让一架无人机在仿真里直接「钻地」了——想想都后怕。
NED坐标系的好处是：数值范围小（通常几公里内），计算方便。我建议在做局部避障和编队飞行时，优先使用NED。
2.1.3 机体坐标系（Body Frame）
机体坐标系是固定在无人机身上的。原点在质心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。
这个坐标系有什么用？说白了，所有传感器（IMU、磁力计、空速管）测量的数据，都是机体坐标系下的。你要把加速度、角速度转换到NED或ECEF，才能做导航。
| 坐标系 | 原点 | 典型用途 | 我踩过的坑 |
|---|---|---|---|
| ECEF | 地心 | 全球导航、长航程规划 | 极地附近精度下降 |
| NED | 起飞点/地面站 | 局部规划、编队控制 | Z轴方向容易搞反 |
| Body | 无人机质心 | 传感器测量、姿态控制 | IMU数据需先校准 |
2.2 坐标变换
坐标变换，说白了就是「翻译」。把机体坐标系下的数据翻译成NED，再把NED翻译成ECEF。翻译的「词典」就是旋转矩阵。
2.2.1 旋转矩阵
三维空间中的旋转，可以用3x3的旋转矩阵表示。绕X轴、Y轴、Z轴的旋转矩阵分别是：
// 绕Z轴旋转（偏航角 ψ）
R_z(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]
// 绕Y轴旋转（俯仰角 θ）
R_y(θ) = [cosθ 0 sinθ]
[0 1 0 ]
[-sinθ 0 cosθ]
// 绕X轴旋转（滚转角 φ）
R_x(φ) = [1 0 0 ]
[0 cosφ -sinφ]
[0 sinφ cosφ]
完整的机体→NED变换，需要按Z-Y-X顺序旋转：
R_b2n = R_z(ψ) * R_y(θ) * R_x(φ)
我的小技巧：写代码时别自己手算旋转矩阵，直接用现成的库（比如Eigen、NumPy的scipy.spatial.transform）。我见过太多人因为手算时符号搞错，导致飞机「翻跟头」。
2.2.2 四元数
旋转矩阵有个问题：万向锁。当俯仰角接近±90°时，偏航和滚转无法区分。这时候，四元数就派上用场了。
四元数 q = [w, x, y, z]，其中 w 是实部，x、y、z 是虚部。它没有奇点，而且插值平滑。我个人习惯在姿态估计和路径平滑时用四元数，只在最终输出时转成欧拉角。
警告：四元数必须归一化！我刚开始做飞控时，忘了归一化，结果姿态估计越飘越远。检查了三天才发现是这个问题。
2.3 刚体运动学模型
有了坐标系和变换，我们终于可以描述无人机的运动了。刚体运动学，说白了就是研究「位置、速度、姿态、角速度」之间的关系。
2.3.1 位置与速度
在NED坐标系下，位置 p = [p_n, p_e, p_d]^T，速度 v = [v_n, v_e, v_d]^T。它们的关系很简单：
p_dot = v
嗯，就这么简单。但要注意：这里的 p_d 是高度（向下为正），而通常我们说的「高度」是向上为正。所以转换时要加个负号。
2.3.2 姿态与角速度
姿态用欧拉角表示：Φ = [φ, θ, ψ]^T。角速度 ω = [p, q, r]^T（机体坐标系下）。
它们的关系稍微复杂一点：
Φ_dot = W(Φ) * ω
其中 W(Φ) = [1, sinφ*tanθ, cosφ*tanθ]
[0, cosφ, -sinφ ]
[0, sinφ/cosθ, cosφ/cosθ]
看到 tanθ 了吗？当 θ = ±90° 时，tanθ 无穷大。这就是万向锁的数学本质。所以，如果你要做全姿态飞行（比如特技无人机），千万别用欧拉角做积分。
2.3.3 完整的运动学模型
把位置和姿态合在一起，就是6自由度刚体运动学模型：
状态量: x = [p_n, p_e, p_d, φ, θ, ψ]^T
输入量: u = [v_n, v_e, v_d, p, q, r]^T
状态方程:
p_dot = v
Φ_dot = W(Φ) * ω
这个模型是路径规划的基础。你想想看，规划器要生成一条轨迹，本质上就是在找一条满足这个运动学约束的路径。
核心思想：路径规划不是画一条线那么简单。你得保证无人机能飞出来——速度不能太大、姿态不能太陡、角速度不能超限。这些约束，都藏在运动学模型里。
2.4 本章小结
好了，我们来捋一捋：
- 三种坐标系：ECEF（全球）、NED（局部）、Body（传感器）
- 坐标变换：旋转矩阵（直观但万向锁）、四元数（无奇点但需归一化）
- 运动学模型：位置-速度、姿态-角速度，6自由度刚体
说实话，这些内容看起来是基础，但我在实际项目中见过太多因为坐标系搞错而炸机的案例。有一次，一个团队把机体坐标系的加速度直接当成NED坐标系的加速度用，结果无人机一飞就偏——还好是在仿真里发现的。
所以，我的建议是：每次做坐标变换时，都画个图确认一下方向。别嫌麻烦，这比炸机后返厂维修省事多了。
公众号:蓝海资料掘金营，微信deep3321