第3章传感器坐标系

3、传感器坐标系：机体坐标系、导航坐标系、地球坐标系、坐标系转换基础

做惯导系统这些年，坐标系这东西，说简单也简单，说复杂真能把你绕晕。我刚开始接触飞控那会儿，就因为在坐标系转换上栽了个跟头——飞机明明朝北飞，导航解算出来的位置却往东偏了十几米。后来一查，原来是机体坐标系和导航坐标系搞混了。

说白了，坐标系就是给传感器数据找个「家」。加速度计测的是哪个方向？陀螺仪转的是绕哪个轴？没有坐标系，数据就是一堆没意义的数字。今天咱们就把这三个最常用的坐标系捋清楚。

机体坐标系是跟着飞机走的。你想想看，飞机在翻滚、俯仰、偏航，它身上的传感器可不知道什么东南西北，它只知道自己的前后左右。

我习惯这样定义：

嗯，这里要注意：不同厂家可能有不同定义。比如有些无人机把Z轴朝上，有些朝下。我在项目中遇到过，IMU模块的坐标系和飞控定义的坐标系不一致，结果姿态解算直接炸了。所以拿到传感器数据，第一件事就是确认它的坐标系定义。

核心要点：机体坐标系是「跟着飞机动」的。加速度计测的是机体坐标系下的加速度，陀螺仪测的是机体坐标系下的角速度。所有传感器原始数据，默认都在这个坐标系里。

导航坐标系是「大地参考系」。说白了，就是告诉飞机：你现在在哪儿，往哪个方向飞。

最常用的是北东地（NED）坐标系：

为什么用NED而不是ENU（东北天）？我个人习惯用NED，因为和飞机的高度方向一致——高度下降时D轴为正，很直观。但如果你做的是地面机器人，ENU可能更顺手。

导航坐标系是「不动」的。飞机在飞，但导航坐标系始终固定在地面上。我们做惯导解算，最终要输出的位置、速度、姿态，都是在导航坐标系下的值。

小技巧：我一般把导航坐标系的原点设在起飞点。这样位置输出就是相对于起飞点的偏移量，调试时特别方便。如果你做的是长航时飞行，记得考虑地球曲率，那就得用ECEF了。

ECEF全称是Earth-Centered Earth-Fixed，地心地固坐标系。它把原点放在地球质心，跟着地球一起转。

定义很简单：

什么时候用ECEF？GPS输出的经纬高，本质上就是ECEF下的坐标。我做组合导航时，经常要把GPS的经纬度转成ECEF，再和惯导的NED坐标融合。这个过程，说白了就是坐标系转换。

注意：ECEF和NED之间的转换，需要知道当前位置的经纬度。我曾经犯过一个错：直接用起飞点的经纬度做转换，结果飞机飞出去10公里后，位置误差已经大到没法看了。正确的做法是实时更新转换矩阵。

坐标系转换，核心就是旋转矩阵。你想想看，一个向量在机体坐标系下是[ax, ay, az]，在导航坐标系下是[AN, AE, AD]，它们之间就差一个旋转。

旋转矩阵R满足：

[AN]     [R11 R12 R13] [ax]
[AE]  =  [R21 R22 R23] [ay]
[AD]     [R31 R32 R33] [az]

这个R矩阵，其实就是由飞机的姿态角（横滚、俯仰、偏航）决定的。我习惯用ZYX顺序旋转：先偏航，再俯仰，最后横滚。

旋转矩阵有个重要性质：它是正交矩阵，逆等于转置。所以从导航系转到机体系，直接用R的转置就行。

避坑指南：我曾经在代码里直接用同一个矩阵做正反转换，结果姿态解算越来越飘。后来才发现，旋转矩阵的转置和逆虽然数学上相等，但数值计算时会有微小误差。建议每次都重新计算，别偷懒。

下面这张图，是我做惯导系统时经常贴在工位上的。它把三个坐标系的关系和转换路径画得清清楚楚：

在实际项目中，我一般按这个流程走：

这里有个细节：陀螺仪测的是角速度，积分后得到姿态角。但姿态角本身就是在导航坐标系下定义的。所以陀螺仪数据其实不需要做坐标系转换，直接积分就行。嗯，这个点很多人会搞混。

我的习惯：在代码里用枚举类型明确标注每个变量的坐标系。比如 acc_body、acc_nav、pos_ecef。这样一眼就能看出数据在哪个坐标系下，调试时少走很多弯路。

好了，坐标系这块就聊到这儿。记住一句话：传感器数据在机体系，导航结果在导航系，GPS在ECEF系。搞清楚了这三个坐标系的关系，惯导系统的基本框架就搭起来了。

公众号:蓝海资料掘金营，微信deep3321