第4章坐标转换算法

第4章坐标转换算法：地心地固系、地理系、载体系之间的转换矩阵

坐标转换，说白了就是给导弹指路。你想想看，导弹在天上飞，它得知道自己在哪里，目标在哪里，自己脑袋朝哪个方向。这三个信息分别对应三个坐标系：地心地固系（ECEF）、地理系（NED）、载体系（Body）。

我刚开始做机载武器系统那会儿，就吃过坐标转换的亏。有一次半实物仿真，导弹打出去偏了十几公里，查了三天，最后发现是转换矩阵里一个符号写反了。嗯，从那以后，我对这块就特别较真。

4.1 三个坐标系，先搞清楚它们长啥样

在动手写代码之前，我建议你先在脑子里把这三个坐标系画出来。坐标系搞混了，后面全是白搭。

4.1.1 地心地固系（ECEF）

这个坐标系的原点在地球质心。Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。说白了，它跟着地球一起转。你在北京站着一个小时不动，你在ECEF系下的坐标基本不变。

4.1.2 地理系（NED）

也叫导航系。原点在载机或导弹的质心。N指向北，E指向东，D指向地心。注意这个D是向下，不是向上。我见过有新手把D搞反了，结果高度算出来是负的。

4.1.3 载体系（Body）

这个最简单。原点在飞行器质心，X轴指向机头，Y轴指向右翼，Z轴指向下。跟地理系一样，Z轴也是向下。为什么都向下？因为这样符合右手定则，而且俯仰角为正时抬头，比较直观。

4.2 转换矩阵，核心就三个

坐标转换的核心，就是三个旋转矩阵。我个人习惯把它们记成：绕X轴转、绕Y轴转、绕Z轴转。任何一个三维旋转，都可以拆成这三个基本旋转的组合。

旋转轴	旋转矩阵（绕该轴旋转θ角）
X轴	[1, 0, 0; 0, cosθ, sinθ; 0, -sinθ, cosθ]
Y轴	[cosθ, 0, -sinθ; 0, 1, 0; sinθ, 0, cosθ]
Z轴	[cosθ, sinθ, 0; -sinθ, cosθ, 0; 0, 0, 1]

注意：旋转矩阵的正负号跟旋转方向定义有关。我用的都是右手定则：拇指指向旋转轴正方向，四指弯曲方向为正。不同教材可能定义不同，一定要先确认。

4.3 从地理系到载体系：最常用的转换

这个转换在火控解算里天天用。地理系到载体系，需要三个角度：航向角ψ、俯仰角θ、横滚角φ。转换顺序是：先航向，再俯仰，最后横滚。

为什么是这个顺序？你想想看，飞机在空中调整姿态，先转方向（航向），再抬头低头（俯仰），最后滚转。这个顺序跟实际物理过程一致。

转换矩阵长这样：

C_ned2body = R_x(φ) * R_y(θ) * R_z(ψ)

其中：
R_z(ψ) = [cosψ, sinψ, 0; -sinψ, cosψ, 0; 0, 0, 1]
R_y(θ) = [cosθ, 0, -sinθ; 0, 1, 0; sinθ, 0, cosθ]
R_x(φ) = [1, 0, 0; 0, cosφ, sinφ; 0, -sinφ, cosφ]

我的经验：实际工程中，航向角ψ通常以地理北为0°，顺时针为正。但有些惯导系统用不同的定义。我曾经遇到过因为航向角定义不一致，导致瞄准线偏差了5个密位。所以拿到数据后，第一件事就是确认角度定义。

4.4 从地心地固系到地理系：位置转换

这个转换需要知道当前点的经纬高（L, λ, h）。转换矩阵是：

C_ecef2ned = 
[-sinλ, cosλ, 0;
 -sinL*cosλ, -sinL*sinλ, cosL;
 cosL*cosλ, cosL*sinλ, sinL]

注意，这个矩阵只负责旋转，不负责平移。因为ECEF和NED的原点不同，所以实际转换时，需要先把ECEF坐标减去原点坐标，再做旋转。

我建议你把这一步封装成一个函数，输入是ECEF坐标和参考点的经纬高，输出是NED坐标。这样调用起来方便，也不容易出错。

4.5 完整的转换链：从ECEF到载体系

实际工程中，我们经常需要把目标从ECEF系转到载体系。完整的链路是：

目标ECEF坐标 → 减去载机ECEF坐标 → 得到相对位置向量
相对位置向量 × C_ecef2ned → 得到NED系下的目标位置
NED系下的目标位置 × C_ned2body → 得到载体系下的目标位置

代码实现大概是这样：

// 伪代码，实际工程中请用double类型
Vector3 targetInBody = C_ned2body * (C_ecef2ned * (targetECEF - ownECEF));

// 或者写成链式调用
Vector3 relECEF = targetECEF - ownECEF;
Vector3 relNED = C_ecef2ned * relECEF;
Vector3 relBody = C_ned2body * relNED;

  核心要点：转换矩阵的乘法顺序不能错。矩阵乘法不满足交换律，先转再乘和先乘再转，结果完全不同。我习惯在代码里用括号明确标出顺序，哪怕不加括号也能算对，但加了括号可读性更好，也方便别人review。

4.6 避坑指南：我踩过的几个坑

做坐标转换，有几个地方特别容易出问题。我一个个说：

角度单位搞混：我曾经把一个弧度当成了度传进去，结果矩阵算出来全是错的。现在我的代码里，所有角度变量名都带后缀，比如psi_rad、theta_deg，一眼就能看出来单位。
旋转顺序搞错：不同系统可能用不同的旋转顺序。比如有些系统用Z-Y-X，有些用Z-X-Y。拿到接口文档后，一定要确认旋转顺序和角度定义。
矩阵求逆搞混：旋转矩阵是正交矩阵，它的逆等于它的转置。但如果你把平移也加进去了，那就不是简单的转置了。我建议旋转和平移分开处理，别混在一起。
精度问题：在嵌入式平台上，float精度可能不够。尤其是经纬度转换时，1e-7度的误差，在地面上就是十几米。我建议所有坐标转换都用double。

4.7 工程实现建议

最后，给几个工程上的建议：

写单元测试：每个转换函数都要写测试。输入一个已知的向量，手动算一遍，跟程序输出对比。我每次改代码都会跑一遍测试，确保没把符号改反。
用结构体封装：把坐标系信息封装成结构体，比如ECEFCoord、NEDCoord、BodyCoord。这样类型安全，不容易传错参数。
加日志输出：在关键转换点加日志，输出转换前后的坐标值。调试的时候，这些日志能帮你快速定位问题。
考虑性能：在嵌入式平台上，矩阵乘法可能比较耗时。如果转换频率很高（比如1000Hz），可以考虑把一些固定矩阵缓存起来，避免重复计算。

坐标转换这块，说难不难，说简单也不简单。关键是要细心，把每个细节都搞清楚。我做了十几年机载武器系统，每次遇到坐标转换问题，还是会老老实实把矩阵手算一遍，确认无误后再写代码。嗯，小心驶得万年船。

📚 课程目录