第三讲：坐标系统与变换——地心坐标系、站心坐标系、载体坐标系，以及坐标旋转矩阵推导
各位同学，咱们今天聊点硬核的。坐标系统，说白了就是给目标“定位”的数学框架。你想想看，火控系统里，雷达测出来的是斜距和角度，红外传感器给出的是像素坐标，激光测距机报的是距离——这些数据怎么揉到一起？靠的就是坐标变换。
我当年刚入行时，吃过一次大亏。某次靶试，雷达和光电数据死活对不上，折腾了一宿才发现是坐标系定义搞混了。嗯，从那以后，我养成了一个习惯：任何项目开始前，先把坐标系约定写清楚。
3.1 地心坐标系（ECEF）
地心坐标系，全称是Earth-Centered Earth-Fixed坐标系。原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。
这个坐标系最大的特点是——它跟着地球一起转。你站在地面上不动，你的ECEF坐标是变化的（因为地球在自转）。但在火控系统中，我们通常用它来做全局定位，比如把卫星导航数据、雷达测量数据统一到一个框架下。
关键参数：
- WGS-84椭球模型：长半轴a = 6378137.0 m，扁率f = 1/298.257223563
- 第一偏心率平方：e² = 2f - f²
- 卯酉圈曲率半径：N = a / sqrt(1 - e²sin²φ)
从经纬高（φ, λ, h）到ECEF（X, Y, Z）的转换公式，我建议你直接背下来：
// 经纬高 → ECEF
double phi = lat * M_PI / 180.0;
double lambda = lon * M_PI / 180.0;
double N = a / sqrt(1 - e2 * sin(phi) * sin(phi));
double X = (N + h) * cos(phi) * cos(lambda);
double Y = (N + h) * cos(phi) * sin(lambda);
double Z = (N * (1 - e2) + h) * sin(phi);
反过来，从ECEF到经纬高，需要迭代计算。我习惯用牛顿法，一般3次迭代就能收敛到毫米级精度。
3.2 站心坐标系（ENU）
站心坐标系，也叫东北天坐标系。原点设在观测站位置，X轴指向东，Y轴指向北，Z轴指向天顶（垂直于椭球面向上）。
为什么要有站心坐标系？因为人在本地操作时，更习惯“东-北-天”这种直观的方位。雷达操作员说“目标方位30°，仰角15°”，这其实就是站心坐标系下的极坐标表示。
从ECEF到ENU的变换，本质上是一个平移加旋转的过程：
- 先把ECEF原点平移到站心位置
- 再绕Z轴旋转（90° - λ），使X轴指向东
- 再绕X轴旋转（90° - φ），使Z轴指向天顶
实战技巧：我在做多传感器融合时，通常把雷达数据先转到ECEF，再转到ENU。这样做的原因是——雷达本身就在站心坐标系下工作，但不同雷达的站心位置不同，统一到ECEF才能做空间对齐。
旋转矩阵长这样（记住，这是从ECEF到ENU）：
R_ecef2enu = [
-sin(lambda), cos(lambda), 0;
-sin(phi)*cos(lambda), -sin(phi)*sin(lambda), cos(phi);
cos(phi)*cos(lambda), cos(phi)*sin(lambda), sin(phi)
];
3.3 载体坐标系（Body Frame）
载体坐标系，就是跟着平台走的坐标系。原点在载体质心，X轴指向载体前方（航向），Y轴指向右侧，Z轴指向下方（符合右手定则）。
你想想看，导弹上的导引头、飞机上的雷达、舰船上的光电转塔——它们都是安装在载体上的。传感器测到的目标位置，首先是在传感器自身的坐标系里，然后要转到载体坐标系，最后才能转到地理坐标系。
注意：不同平台对载体坐标系的定义可能不同。比如航空领域常用“前-右-下”，而航海领域可能用“前-左-上”。我建议你在项目文档里，用一张图把坐标系定义画清楚，避免歧义。
载体坐标系到站心坐标系的转换，靠的是姿态角（航向角ψ、俯仰角θ、横滚角φ）。旋转顺序我习惯用Z-Y-X（航向→俯仰→横滚），这是航空领域的标准顺序。
3.4 坐标旋转矩阵推导
好，到了最核心的部分。坐标旋转矩阵，说白了就是“怎么把一个向量从一个坐标系转到另一个坐标系”。
我们先从二维开始。假设一个点P在坐标系A中的坐标为(x, y)，坐标系B相对于A绕Z轴旋转了角度θ。那么P在B中的坐标是：
x' = x*cosθ + y*sinθ
y' = -x*sinθ + y*cosθ
写成矩阵形式：
Rz(θ) = [
cosθ, sinθ, 0;
-sinθ, cosθ, 0;
0, 0, 1
];
同理，绕X轴和Y轴的旋转矩阵：
Rx(φ) = [
1, 0, 0;
0, cosφ, sinφ;
0, -sinφ, cosφ
];
Ry(θ) = [
cosθ, 0, -sinθ;
0, 1, 0;
sinθ, 0, cosθ
];
推导要点：
- 旋转矩阵是正交矩阵，逆矩阵等于转置矩阵
- 多次旋转的复合矩阵，按旋转顺序从右向左乘
- 注意旋转方向：右手定则，拇指指向旋转轴正方向，四指弯曲方向为正
举个例子，从载体坐标系到站心坐标系的旋转矩阵：
R_body2enu = Rz(-ψ) * Ry(-θ) * Rx(-φ);
// 注意：这里取负号是因为我们定义的是“载体坐标系到地理坐标系”的变换
// 而姿态角是“地理坐标系到载体坐标系”的欧拉角
我曾经在项目中遇到过一个问题：用四元数算出来的姿态和欧拉角算出来的对不上。后来发现是旋转顺序搞反了。嗯，这里要提醒大家——欧拉角的旋转顺序一定要和你的传感器输出一致。IMU输出的姿态角，通常都是按Z-Y-X顺序的。
3.5 实战中的坐标系管理
说了这么多理论，咱们来点实际的。在多传感器融合火控系统中，我建议你建立这样的坐标系管理策略：
| 传感器 | 原始坐标系 | 中间坐标系 | 最终坐标系 |
|---|---|---|---|
| 雷达 | 雷达天线坐标系 | 载体坐标系 | 站心坐标系 |
| 光电 | 光电转塔坐标系 | 载体坐标系 | 站心坐标系 |
| 卫星导航 | ECEF | 站心坐标系 | 站心坐标系 |
| 激光测距 | 激光器坐标系 | 光电转塔坐标系 | 载体坐标系 |
你看，所有数据最终都统一到站心坐标系。为什么？因为火控解算需要的是目标相对于本地的位置和速度，站心坐标系最直观。
避坑指南：我曾经在集成测试时发现，雷达和光电的目标航迹总是差几米。查了半天，原来是雷达的安装位置偏移没补偿。记住：传感器到载体的安装偏差（杆臂效应）一定要做补偿，否则融合出来的目标位置会有系统性误差。
最后，给大家一个建议：写代码时，把坐标系变换封装成独立的函数或类。每个变换函数都要写单元测试，用已知的输入输出验证。我习惯用MATLAB先做仿真验证，再移植到C++代码里。这样能省掉很多调试时间。
好了，这一讲的内容就到这里。坐标变换是火控系统的基本功，看似简单，但细节很多。下一讲我们会聊时间同步——另一个让新手头疼的问题。