做无人机开发，绕不开的第一道坎就是坐标系。GPS 给的是经纬度，IMU 给的是机体角速度，飞控要的是导航系速度，相机吐的是像素坐标，云台还有自己的一套……稍不留神符号一反、轴序一错，姿态就飞了。

这篇文章把无人机涉及的坐标系一次性梳理清楚，包含定义、图示、转换矩阵、代码实现，以及一堆我踩过的坑。

---

目录

• 一、为什么需要这么多坐标系
• 二、坐标系全景图
• 三、地球相关坐标系
• 四、导航坐标系（NED / ENU）
• 五、机体坐标系（Body Frame）
• 六、气流坐标系与航迹坐标系
• 七、传感器坐标系：IMU / 云台 / 相机 / 像素
• 八、姿态表示方法：欧拉角、DCM、四元数
• 九、完整转换链路与代码实现
• 十、十个常见的坑

---

一、为什么需要这么多坐标系

本质原因只有一个：不同的物理量，在不同的参考系下描述才最简单。

物理量	传感器	天然所在坐标系
角速度 $\omega$	陀螺仪	机体系（传感器固连在机体上）
比力 $f$	加速度计	机体系
位置	GNSS	大地坐标系（经纬高）
航向	磁力计	机体系测量，需转到导航系
气压高度	气压计	沿重力方向，导航系 z 轴
目标像素	相机	像素坐标系
攻角/侧滑角	空速管/风标	气流坐标系

飞控要做的事，就是把这些散落在各个坐标系的量，统一到同一个坐标系下做融合与控制。所以坐标系转换是导航算法的地基，地基不稳，上面写再漂亮的 EKF 也是白搭。

---

二、坐标系全景图

先给一张总览，后面逐个展开：

记号约定（全文统一）：

• $C_b^n$ 表示从 b 系到 n 系的方向余弦矩阵（DCM），即 $v^n=C_b^n{v}^b$
• 旋转矩阵是正交阵，因此 $C_n^b=(C_b^n{)}^{\mathrm{T}}=(C_b^n{)}^{-1}$
• 链式法则：$C_b^e=C_n^e{C}_b^n$，上下标"消消乐"，中间的抵消

这个记号极其好用，写公式时看上下标能不能对消，就知道有没有写反。

---

三、地球相关坐标系

3.1 地心惯性坐标系（ECI，i 系）

• 原点：地球质心
• $Z_i$ 轴：沿地球自转轴指向北极
• $X_i$ 轴：指向春分点（J2000 历元）
• $Y_i$ 轴：右手定则补齐

特点：不随地球自转，是牛顿力学意义上的近似惯性系。惯导的力学编排方程最原始的形式就写在 i 系里，因为只有在惯性系下 $F=ma$ 才不需要引入附加的科氏力、离心力。

工程上，除非你做航天或者高精度惯导推导，日常无人机开发几乎不直接用 i 系，但它是理解科氏力修正项的来源。

3.2 地心地固坐标系（ECEF，e 系）

• 原点：地球质心
• $Z_e$ 轴：指向北极（与 $Z_i$ 重合）
• $X_e$ 轴：指向本初子午线与赤道的交点
• $Y_e$ 轴：右手定则（指向东经 90°）

e 系随地球一起转，相对 i 系的角速度为：

$${\omega }_{ie}=7.292115\times 10^{-5}rad/s$$

i 系到 e 系只差一个绕 Z 轴的转动：

$$C_i^e=\left[\begin{array}{ccc}\cos ({\omega }_{ie}t)& \sin ({\omega }_{ie}t)& 0\\ -\sin ({\omega }_{ie}t)& \cos ({\omega }_{ie}t)& 0\\ 0& 0& 1\end{array}\right]$$

ECEF 是 GNSS 解算的原生坐标系——你的 RTK 板卡内部算的就是 XYZ，只是输出时帮你转成了经纬高。

3.3 大地坐标系（LLA / Geodetic）

用纬度 $L$、经度 $\lambda$、大地高 $h$ 描述位置，基于 WGS-84 参考椭球：

参数	符号	数值
长半轴	$a$	6378137.0 m
扁率	$f$	1/298.257223563
第一偏心率平方	$e^2$	$2f-f^2\approx 0.00669438$

LLA → ECEF（有闭式解）：

$$\{\begin{array}{l}{x}_e=(R_N+h)\cos L\cos \lambda \\ y_e=(R_N+h)\cos L\sin \lambda \\ z_e=\left[R_N(1-e^2)+h\right]\sin L\end{array}$$

其中卯酉圈曲率半径：

$$R_N=\frac{a}{\sqrt{1-e^2{\sin }^{2}L}}$$

ECEF → LLA 没有简洁的闭式解（因为 $L$ 出现在 $R_N$ 里），经度可以直接算：

$$\lambda =\arctan 2(y_e,x_e)$$

纬度和高度需要迭代，或者用 Bowring / Ferrari 等近似闭式算法。迭代法通常 3~4 次就收敛到毫米级：

$$L_{k+1}=\arctan \left[\frac{z_e+e^2{R}_N(L_k)\sin L_k}{\sqrt{x_e^2+y_e^2}}\right],h=\frac{\sqrt{x_e^2+y_e^2}}{\cos L}-R_N$$

⚠️ 大地高 ≠ 海拔高。GNSS 输出的 $h$ 是相对 WGS-84 椭球面的椭球高，而地图上的海拔是相对大地水准面（Geoid）的正高。二者相差一个大地水准面差距 $N$，在中国大陆大约 -10 m 到 -40 m（华北约 -10 m，青藏高原可达 -40 m 以上）。做贴地飞行、地形跟随时，这个偏差足以撞山。

---

四、导航坐标系（NED / ENU）

导航系（n 系）也叫当地水平坐标系、地理坐标系，原点取在载体所在位置（或某个起飞参考点），坐标轴与当地水平面对齐。

有两种常见约定：

4.1 NED（North-East-Down，北东地）

• $X_n$：指向地理北
• $Y_n$：指向东
• $Z_n$：指向地心（向下）

使用者：PX4 内部、传统惯导、航空航天教材、NASA/AIAA 惯例。

优点：$Z$ 轴向下，重力是正的（$g^n=[0,0,+9.8{]}^{\mathrm{T}}$），高度增加是负的 $Z$。配合前右下（FRD）机体系，构成右手系，且欧拉角定义最自然。

4.2 ENU（East-North-Up，东北天）

• $X_n$：指向东
• $Y_n$：指向北
• $Z_n$：指向天（向上）

使用者：ROS/ROS2（REP-103 强制规定）、MAVROS、大多数 SLAM 框架、测绘领域。

优点：符合"高度往上加"的直觉，跟数学上的 $xy$ 平面 + $z$ 向上一致。

4.3 两者的转换

NED ↔ ENU 是一个对合变换（自己是自己的逆）：

$$C_{\text{NED}}^{\text{ENU}}=C_{\text{ENU}}^{\text{NED}}=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]$$

即：交换前两个分量，第三个取负。注意这个矩阵的行列式是 -1，它不是旋转矩阵而是"旋转 + 反射"——所以不能简单地用欧拉角来表达 NED→ENU 的关系，必须成对地同时切换机体系约定（FRD ↔ FLU），才能保持右手性。

4.4 ECEF → NED

在参考点 $(L_0,{\lambda }_0)$ 处：

$$C_e^n=\left[\begin{array}{ccc}-\sin L_0\cos {\lambda }_0& -\sin L_0\sin {\lambda }_0& \cos L_0\\ -\sin {\lambda }_0& \cos {\lambda }_0& 0\\ -\cos L_0\cos {\lambda }_0& -\cos L_0\sin {\lambda }_0& -\sin L_0\end{array}\right]$$

完整流程（局部切平面 / LTP）：

$$\left[\begin{array}{c}N\\ E\\ D\end{array}\right]=C_e^n\left({\mathbf{p}}_e-{\mathbf{p}}_{e,0}\right)$$

小范围简化：飞行半径几公里以内，可以直接用平面近似，误差在厘米级：

$$\{\begin{array}{l}N\approx (L-L_0)\cdot (R_M+h)\\ E\approx (\lambda -{\lambda }_0)\cdot (R_N+h)\cos L_0\\ D\approx -(h-h_0)\end{array}$$

其中子午圈曲率半径 $R_M=\frac{a(1-e^2)}{(1-e^2{\sin }^{2}L{)}^{3/2}}$。

粗略记：中国纬度下，1° 纬度 ≈ 111 km，1° 经度 ≈ 111 × cos(纬度) km。

---

五、机体坐标系（Body Frame）

原点在无人机重心，与机体固连，随飞机一起动。同样有两种约定：

5.1 FRD（Front-Right-Down，前右下）

• $X_b$：机头方向
• $Y_b$：右翼方向
• $Z_b$：机腹方向（向下）

使用者：PX4 内部、航空航天惯例，与 NED 配对。

5.2 FLU（Front-Left-Up，前左上）

• $X_b$：机头方向
• $Y_b$：左翼方向
• $Z_b$：机背方向（向上）

使用者：ROS（REP-103）、MAVROS 对外接口，与 ENU 配对。

FRD ↔ FLU 转换矩阵：$\mathrm{diag}(1,-1,-1)$，即绕 $X$ 轴转 180°，这个是真正的旋转（行列式 +1）。

5.3 机体系下的状态量

符号	含义	说明
$u,v,w$	机体系三轴速度	前向、右向、下向
$p,q,r$	机体系三轴角速度	滚转率、俯仰率、偏航率
$\varphi ,\theta ,\psi$	欧拉角	滚转 Roll、俯仰 Pitch、偏航 Yaw

角速度的正方向按右手定则：

• $p>0$：右翼下沉（右滚）
• $q>0$：机头上抬（抬头）
• $r>0$：机头右偏

5.4 NED → Body：欧拉角与旋转顺序

航空领域标准采用 Z-Y-X 顺序的内旋（intrinsic），也记作 3-2-1 顺序：

1. 先绕 $Z_n$ 转偏航角 $\psi$
2. 再绕新的 $Y$ 轴转俯仰角 $\theta$
3. 最后绕新的 $X$ 轴转滚转角 $\varphi$

$$C_n^b=R_x(\varphi )R_y(\theta )R_z(\psi )$$

其中（注意这是坐标系旋转/被动旋转的形式）：

$$R_z(\psi )=\left[\begin{array}{ccc}c\psi & s\psi & 0\\ -s\psi & c\psi & 0\\ 0& 0& 1\end{array}\right],R_y(\theta )=\left[\begin{array}{ccc}c\theta & 0& -s\theta \\ 0& 1& 0\\ s\theta & 0& c\theta \end{array}\right],R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& c\varphi & s\varphi \\ 0& -s\varphi & c\varphi \end{array}\right]$$

展开得到完整的 DCM：

$$C_n^b=\left[\begin{array}{ccc}c\theta c\psi & c\theta s\psi & -s\theta \\ s\varphi s\theta c\psi -c\varphi s\psi & s\varphi s\theta s\psi +c\varphi c\psi & s\varphi c\theta \\ c\varphi s\theta c\psi +s\varphi s\psi & c\varphi s\theta s\psi -s\varphi c\psi & c\varphi c\theta \end{array}\right]$$

反过来 $C_b^n=(C_n^b{)}^{\mathrm{T}}$。

从 DCM 反解欧拉角（记 $C_b^n$ 的元素为 $c_{ij}$，$i,j$ 从 1 开始）：

$$\theta =-\arcsin (c_{31}),\varphi =\arctan 2(c_{32},c_{33}),\psi =\arctan 2(c_{21},c_{11})$$

注意：这里用的是 $C_b^n$。如果你手上是 $C_n^b$，公式里的下标要相应转置。务必先确认自己手上的矩阵是哪个方向的，这是最高频的 bug 来源之一。

角度定义域：$\varphi \in (-180°,180°]$，$\theta \in [-90°,90°]$，$\psi \in [0°,360°)$ 或 $(-180°,180°]$。

5.5 欧拉角速率 ≠ 机体角速度

这是新手最容易搞混的一点。陀螺仪测的 $[p,q,r]$ 是机体系下的角速度矢量分量，而 $[\dot{\varphi },\dot{\theta },\dot{\psi }]$ 是三个欧拉角各自的变化率，它们分属三个不同的中间坐标系，不能直接画等号。

转换关系：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

只有在小角度（$\varphi \approx \theta \approx 0$）时才近似有 $\dot{\varphi }\approx p,\dot{\theta }\approx q,\dot{\psi }\approx r$。这也是为什么多旋翼小角度线性化控制器能工作、而做大机动时必须用四元数的原因。

看这个矩阵，当 $\theta \to \pm 90°$ 时 $\tan \theta \to \infty$、$\cos \theta \to 0$，矩阵奇异——这就是万向节死锁（Gimbal Lock）。

---

六、气流坐标系与航迹坐标系

这两个坐标系主要用在固定翼上，多旋翼一般不太关心，但做抗风控制或空速融合时会用到。

6.1 气流坐标系（Wind / Air-path Frame，a 系）

• $X_a$：沿空速矢量方向（飞机相对空气的运动方向）
• $Z_a$：在飞机对称面内，垂直 $X_a$ 向下
• $Y_a$：右手定则

气动力天然定义在这个系里：阻力 D 沿 $-X_a$，升力 L 沿 $-Z_a$，侧力 Y 沿 $Y_a$。

攻角 $\alpha$ 与侧滑角 $\beta$ 定义了 a 系与 b 系的关系。设机体系下空速分量为 $(u,v,w)$：

$$V=\sqrt{u^2+v^2+w^2},\alpha =\arctan \frac{w}{u},\beta =\arcsin \frac{v}{V}$$

反过来：

$$u=V\cos \alpha \cos \beta ,v=V\sin \beta ,w=V\sin \alpha \cos \beta$$

转换矩阵：

$$C_a^b=\left[\begin{array}{ccc}\cos \alpha \cos \beta & -\cos \alpha \sin \beta & -\sin \alpha \\ \sin \beta & \cos \beta & 0\\ \sin \alpha \cos \beta & -\sin \alpha \sin \beta & \cos \alpha \end{array}\right]$$

6.2 航迹坐标系（Trajectory / Path Frame，k 系）

• $X_k$：沿地速矢量方向
• $Z_k$：在包含 $X_k$ 的铅垂面内向下
• $Y_k$：右手定则

与导航系的关系由航迹倾角 $\gamma$（爬升角） 和 航迹方位角 $\chi$ 描述：

$$\gamma =\arcsin \frac{-v_D}{|\mathbf{v}|},\chi =\arctan 2(v_E,v_N)$$

6.3 空速、地速与风速

三者是矢量关系（在同一坐标系下相加）：

$${\mathbf{V}}_{\text{地速}}={\mathbf{V}}_{\text{空速}}+{\mathbf{V}}_{\text{风速}}$$

无风时，气流系与航迹系重合，$\gamma =\theta -\alpha$（无侧滑无滚转的情况）。有风时两者分离，这正是固定翼定点盘旋会画出"椭圆"、多旋翼悬停会有姿态倾角的原因。

---

七、传感器坐标系：IMU / 云台 / 相机 / 像素

7.1 IMU 坐标系

理论上 IMU 应与机体系重合，实际上：

1. 安装位置偏移（杆臂 lever arm）：IMU 不在重心，会引入向心加速度和角加速度切向分量：

$$f_{\text{IMU}}=f_{\text{CG}}+\dot{\mathbf{\omega }}\times \mathbf{r}+\mathbf{\omega }\times (\mathbf{\omega }\times \mathbf{r})$$

高机动时这一项不可忽略，$\mathbf{r}=5\mathrm{cm}$、$\omega =200°/\mathrm{s}$ 时，向心项就有约 $0.6m/{\mathrm{s}}^2$。

2. 安装角误差：焊接、贴片总有 1~3° 偏差，需要通过标定得到固定的安装矩阵 $C_{\text{IMU}}^b$。

3. GNSS 天线杆臂：GNSS 天线也不在重心，位置需要补偿 ${\mathbf{p}}_{\text{CG}}={\mathbf{p}}_{\text{ant}}-C_b^n{\mathbf{r}}_{\text{ant}}^b$。松组合做得再好，杆臂不补偿，转弯时位置就会甩出去。

7.2 云台坐标系

三轴云台通过 Yaw-Pitch-Roll 三个电机隔离机体运动。注意云台角度有两种口径：

• 关节角（joint angle）：相对机体的角度，电机编码器直接读出
• 姿态角（attitude angle）：相对导航系的绝对角度，云台 IMU 输出

$$C_g^n=C_b^n\cdot C_g^b$$

其中 $C_g^b$ 由三个关节角构成。做目标定位时一定要用绝对姿态角，否则机体一晃目标就跑了。

7.3 相机坐标系

OpenCV / 计算机视觉惯例（RDF）：

• $X_c$：图像右
• $Y_c$：图像下
• $Z_c$：光轴向前

注意这跟机体系 FRD 差了一个旋转：相机的"前"是 $Z$，机体的"前"是 $X$。典型的前视相机安装：

$$C_c^b=\left[\begin{array}{ccc}0& 0& 1\\ 1& 0& 0\\ 0& 1& 0\end{array}\right]$$

（含义：相机 $Z$ → 机体 $X$，相机 $X$ → 机体 $Y$，相机 $Y$ → 机体 $Z$）

7.4 图像与像素坐标系

• 图像坐标系：原点在光轴与像平面交点（主点），单位是毫米
• 像素坐标系：原点在图像左上角，$u$ 向右、$v$ 向下，单位是像素

针孔模型：

$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\underset{\text{内参 }K}{\underbrace{\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]$$

7.5 完整的目标定位链路

无人机看到地面一个目标点，算它的经纬度，要走完这一整条链：

$$\text{像素}(u,v)\stackrel{K^{-1}}{\to }\text{相机系}\stackrel{C_c^g}{\to }\text{云台系}\stackrel{C_g^b}{\to }\text{机体系}\stackrel{C_b^n}{\to }\text{NED}\stackrel{C_n^e}{\to }\text{ECEF}\to \text{LLA}$$

由于单目缺少深度，最后一步通常用射线与地平面求交来定深：设相机在 NED 下位置为 $(N_c,E_c,D_c)$，射线方向单位矢量为 ${\mathbf{d}}^n=[d_N,d_E,d_D{]}^{\mathrm{T}}$，地面高度为 $D_g$，则：

$$s=\frac{D_g-D_c}{d_D},{\mathbf{p}}_{\text{target}}^n={\mathbf{p}}_c^n+s{\mathbf{d}}^n$$

要求 $d_D>0$（射线朝下），否则说明相机在看天，无解。

---

八、姿态表示方法：欧拉角、DCM、四元数

方法	参数量	优点	缺点
欧拉角	3	直观，物理意义明确	万向节死锁，三角函数开销大，插值非线性
方向余弦矩阵	9	无奇异，变换直接	冗余度高，需要正交化，存储/计算成本高
四元数	4	无奇异，计算高效，插值平滑	不直观，有双覆盖（$q$ 与 $-q$ 表示同一姿态）
旋转矢量	3	无冗余，适合圆锥误差补偿	$2\pi$ 处奇异

工程实践：内部用四元数解算，对外显示用欧拉角，需要变换矢量时转成 DCM。

8.1 四元数与 DCM

定义 $\mathbf{q}=[q_0,q_1,q_2,q_3{]}^{\mathrm{T}}$（Hamilton 约定，标量在前），$\Vert \mathbf{q}\Vert =1$：

$$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

8.2 四元数与欧拉角

欧拉角 → 四元数（Z-Y-X 顺序）：

$$\{\begin{array}{l}{q}_0=c\frac{\varphi }{2}c\frac{\theta }{2}c\frac{\psi }{2}+s\frac{\varphi }{2}s\frac{\theta }{2}s\frac{\psi }{2}\\ q_1=s\frac{\varphi }{2}c\frac{\theta }{2}c\frac{\psi }{2}-c\frac{\varphi }{2}s\frac{\theta }{2}s\frac{\psi }{2}\\ q_2=c\frac{\varphi }{2}s\frac{\theta }{2}c\frac{\psi }{2}+s\frac{\varphi }{2}c\frac{\theta }{2}s\frac{\psi }{2}\\ q_3=c\frac{\varphi }{2}c\frac{\theta }{2}s\frac{\psi }{2}-s\frac{\varphi }{2}s\frac{\theta }{2}c\frac{\psi }{2}\end{array}$$

8.3 四元数运动学方程

姿态更新的核心方程：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_{nb}^b\end{array}\right]=\frac{1}{2}\mathbf{\Omega }(\mathbf{\omega })\mathbf{q}$$

其中：

$$\mathbf{\Omega }(\mathbf{\omega })=\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]$$

注意 ${\mathbf{\omega }}_{nb}^b$ 是机体相对导航系的角速度，而陀螺测的是机体相对惯性系的 ${\mathbf{\omega }}_{ib}^b$，二者相差地球自转和位置移动引起的牵连角速度：

$${\mathbf{\omega }}_{nb}^b={\mathbf{\omega }}_{ib}^b-C_n^b({\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)$$

对于消费级 MEMS 陀螺（零偏漂移 $>10°/\mathrm{h}$），地球自转 $15°/\mathrm{h}$ 完全淹没在噪声里，可以直接忽略；但对于战术级以上的光纤陀螺，这一项必须补偿。

---

九、完整转换链路与代码实现

下面是一份可直接使用的 Python 实现（NED / FRD 约定）：

import numpy as np

# ---------- WGS-84 参数 ----------
A_WGS84 = 6378137.0
F_WGS84 = 1.0 / 298.257223563
E2_WGS84 = F_WGS84 * (2 - F_WGS84)


def lla2ecef(lat_deg, lon_deg, h):
    """大地坐标 -> ECEF，角度输入单位为度，高度为米（椭球高）"""
    lat, lon = np.radians(lat_deg), np.radians(lon_deg)
    sL, cL = np.sin(lat), np.cos(lat)
    RN = A_WGS84 / np.sqrt(1 - E2_WGS84 * sL ** 2)
    x = (RN + h) * cL * np.cos(lon)
    y = (RN + h) * cL * np.sin(lon)
    z = (RN * (1 - E2_WGS84) + h) * sL
    return np.array([x, y, z])


def ecef2lla(p, tol=1e-12, max_iter=10):
    """ECEF -> 大地坐标，迭代法"""
    x, y, z = p
    lon = np.arctan2(y, x)
    r = np.hypot(x, y)
    lat = np.arctan2(z, r * (1 - E2_WGS84))  # 初值
    for _ in range(max_iter):
        sL = np.sin(lat)
        RN = A_WGS84 / np.sqrt(1 - E2_WGS84 * sL ** 2)
        h = r / np.cos(lat) - RN
        lat_new = np.arctan2(z, r * (1 - E2_WGS84 * RN / (RN + h)))
        if abs(lat_new - lat) < tol:
            lat = lat_new
            break
        lat = lat_new
    sL = np.sin(lat)
    RN = A_WGS84 / np.sqrt(1 - E2_WGS84 * sL ** 2)
    h = r / np.cos(lat) - RN
    return np.degrees(lat), np.degrees(lon), h


def C_e2n(lat_deg, lon_deg):
    """ECEF -> NED 的方向余弦矩阵"""
    lat, lon = np.radians(lat_deg), np.radians(lon_deg)
    sL, cL, sl, cl = np.sin(lat), np.cos(lat), np.sin(lon), np.cos(lon)
    return np.array([
        [-sL * cl, -sL * sl,  cL],
        [-sl,       cl,       0.0],
        [-cL * cl, -cL * sl, -sL],
    ])


def lla2ned(lat, lon, h, lat0, lon0, h0):
    """相对参考点的 NED 局部坐标"""
    return C_e2n(lat0, lon0) @ (lla2ecef(lat, lon, h) - lla2ecef(lat0, lon0, h0))


def euler2dcm(roll, pitch, yaw):
    """欧拉角(弧度) -> C_n^b，Z-Y-X 内旋顺序"""
    sr, cr = np.sin(roll), np.cos(roll)
    sp, cp = np.sin(pitch), np.cos(pitch)
    sy, cy = np.sin(yaw), np.cos(yaw)
    return np.array([
        [cp * cy,                cp * sy,                -sp],
        [sr * sp * cy - cr * sy, sr * sp * sy + cr * cy,  sr * cp],
        [cr * sp * cy + sr * sy, cr * sp * sy - sr * cy,  cr * cp],
    ])


def dcm2euler(C_b2n):
    """C_b^n -> 欧拉角(弧度)，含万向节死锁保护"""
    if abs(C_b2n[2, 0]) > 0.99999:              # |sin(pitch)| ≈ 1
        pitch = np.sign(-C_b2n[2, 0]) * np.pi / 2
        roll = 0.0                              # 死锁时 roll/yaw 不可分，约定 roll=0
        yaw = np.arctan2(-C_b2n[0, 1], C_b2n[1, 1])
        return roll, pitch, yaw
    pitch = -np.arcsin(C_b2n[2, 0])
    roll = np.arctan2(C_b2n[2, 1], C_b2n[2, 2])
    yaw = np.arctan2(C_b2n[1, 0], C_b2n[0, 0])
    return roll, pitch, yaw


def euler2quat(roll, pitch, yaw):
    """欧拉角 -> 四元数 [w, x, y, z]"""
    cr, sr = np.cos(roll / 2), np.sin(roll / 2)
    cp, sp = np.cos(pitch / 2), np.sin(pitch / 2)
    cy, sy = np.cos(yaw / 2), np.sin(yaw / 2)
    return np.array([
        cr * cp * cy + sr * sp * sy,
        sr * cp * cy - cr * sp * sy,
        cr * sp * cy + sr * cp * sy,
        cr * cp * sy - sr * sp * cy,
    ])


def quat2dcm(q):
    """四元数 [w,x,y,z] -> C_b^n"""
    w, x, y, z = q / np.linalg.norm(q)
    return np.array([
        [1 - 2 * (y*y + z*z), 2 * (x*y - w*z),     2 * (x*z + w*y)],
        [2 * (x*y + w*z),     1 - 2 * (x*x + z*z), 2 * (y*z - w*x)],
        [2 * (x*z - w*y),     2 * (y*z + w*x),     1 - 2 * (x*x + y*y)],
    ])


def quat_propagate(q, omega, dt):
    """四元数一阶积分更新，omega = [p,q,r] (rad/s)"""
    p_, q_, r_ = omega
    Omega = np.array([
        [0,  -p_, -q_, -r_],
        [p_,  0,   r_, -q_],
        [q_, -r_,  0,   p_],
        [r_,  q_, -p_,  0 ],
    ])
    q_new = q + 0.5 * Omega @ q * dt
    return q_new / np.linalg.norm(q_new)      # 必须归一化！


NED2ENU = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])   # 自逆
FRD2FLU = np.diag([1.0, -1.0, -1.0])                     # 自逆


# ---------- 端到端示例：像素 -> 地面目标经纬度 ----------
def pixel_to_ground(u, v, K, C_c2b, att_rpy, drone_lla, ref_lla, ground_h=0.0):
    """
    单目 + 平地假设的目标定位
    K:        相机内参 3x3
    C_c2b:    相机系 -> 机体系 的安装矩阵
    att_rpy:  无人机欧拉角 (roll, pitch, yaw)，弧度
    drone_lla/ref_lla: (lat, lon, h)
    """
    # 1) 像素 -> 相机系归一化射线
    d_c = np.linalg.inv(K) @ np.array([u, v, 1.0])
    d_c /= np.linalg.norm(d_c)

    # 2) 相机系 -> 机体系 -> NED
    C_b2n = euler2dcm(*att_rpy).T
    d_n = C_b2n @ C_c2b @ d_c

    if d_n[2] <= 1e-6:
        raise ValueError("射线未指向地面（相机在看天或平视）")

    # 3) 与地平面求交
    p_c_ned = lla2ned(*drone_lla, *ref_lla)
    D_ground = -(ground_h - ref_lla[2])
    s = (D_ground - p_c_ned[2]) / d_n[2]
    p_tgt_ned = p_c_ned + s * d_n

    # 4) NED -> ECEF -> LLA
    p_tgt_ecef = lla2ecef(*ref_lla) + C_e2n(ref_lla[0], ref_lla[1]).T @ p_tgt_ned
    return ecef2lla(p_tgt_ecef)

自检代码（写完转换函数一定要跑一遍这个）：

# 往返一致性
lla = (39.9042, 116.4074, 50.0)
assert np.allclose(ecef2lla(lla2ecef(*lla)), lla, atol=1e-6)

# 欧拉角往返
rpy = (0.3, -0.2, 1.1)
assert np.allclose(dcm2euler(euler2dcm(*rpy).T), rpy, atol=1e-9)

# 四元数与 DCM 一致性
assert np.allclose(quat2dcm(euler2quat(*rpy)), euler2dcm(*rpy).T, atol=1e-9)

# 正交性
C = euler2dcm(*rpy)
assert np.allclose(C @ C.T, np.eye(3), atol=1e-12)
assert np.isclose(np.linalg.det(C), 1.0)

# 重力向量：水平静止时，机体系加速度计应读到 [0,0,g] (FRD/NED)
g_n = np.array([0, 0, 9.8])
assert np.allclose(euler2dcm(0, 0, 0) @ g_n, [0, 0, 9.8])

# 抬头 30° 时，重力在机体 x 轴有 -g*sin(30°) 分量
print(euler2dcm(0, np.radians(30), 0) @ g_n)   # [-4.9, 0, 8.49]

最后一条特别有用：用重力向量做 sanity check，可以一眼看出符号是否搞反。

---

十、十个常见的坑

1️⃣ NED 和 ENU 混用

最经典的坑。PX4 内部 NED/FRD，MAVROS 对外 ENU/FLU，两边一对接就出问题。记住：坐标系约定必须成对切换（NED↔ENU 的同时 FRD↔FLU），否则右手系变左手系，姿态全乱。

调试口诀：让飞机原地向东平移，看输出的第一个分量。是正的 → ENU；接近 0 而第二个是正的 → NED。

2️⃣ $C_b^n$ 和 $C_n^b$ 搞反

写代码时把方向余弦矩阵变量名写全，比如 C_body_to_ned，而不是 C 或者 R。多打十几个字符，能省几小时调试。

3️⃣ 主动旋转 vs 被动旋转

• 主动旋转：坐标系不动，把矢量转过去
• 被动旋转：矢量不动，把坐标系转过去

两者互为转置。本文用的全是被动旋转（航空惯例），而 scipy.spatial.transform.Rotation 默认返回的是主动旋转矩阵。用 scipy 时注意：

from scipy.spatial.transform import Rotation as R
# scipy 的 'ZYX' + degrees，返回的 as_matrix() 是 C_b^n（主动）
C_b2n = R.from_euler('ZYX', [yaw, pitch, roll]).as_matrix()
C_n2b = C_b2n.T

4️⃣ 四元数存储顺序：wxyz vs xyzw

• Hamilton + wxyz：Eigen（构造函数）、MATLAB、PX4、多数惯导教材
• xyzw：ROS geometry_msgs/Quaternion、scipy、Eigen 的内存布局（coeffs()）

Eigen 尤其阴间：Quaterniond q(w, x, y, z) 构造是 wxyz，但 q.coeffs() 返回的是 xyzw。从裸内存 memcpy 的时候一定小心。

另外还有 JPL 约定（乘法顺序相反），航天领域文献常用，跟 Hamilton 混着看会精神分裂。

5️⃣ 万向节死锁

俯仰角 $\theta \to \pm 90°$ 时，roll 和 yaw 无法区分。多旋翼平飞时到不了，但穿越机翻滚、固定翼失速、云台俯冲拍摄都会撞上。

解决方案：内部姿态解算一律用四元数；仅在最后显示给用户时转欧拉角，并对 $\theta =\pm 90°$ 做特殊处理（见上面代码）。

6️⃣ 磁北 vs 真北

磁力计测的是磁北，导航系定义的是真北（地理北），二者相差磁偏角（Declination）。

中国境内磁偏角从东北的 -10° 左右变化到西部的 +2° 左右，而且逐年漂移。飞控通常用 WMM 模型根据经纬度查表补偿。带 RTK 双天线定向的机型可以直接测真北，精度和抗磁干扰能力都远好于磁罗盘。

7️⃣ 椭球高 vs 海拔高

前面提过：GNSS 的高度是椭球高，地图海拔是正高，差一个大地水准面差距。做地形跟随、贴地飞行、航测 DEM 配准时必须转换。

8️⃣ 杆臂效应

IMU 和 GNSS 天线都不在重心。IMU 杆臂在高机动时引入伪加速度，GNSS 杆臂在姿态变化时引入位置跳变。松/紧组合导航中不补偿杆臂，转弯时轨迹会呈现规律性外扩。

9️⃣ 四元数忘记归一化

数值积分会让 $\Vert \mathbf{q}\Vert$ 慢慢偏离 1，导致等效的旋转矩阵不再正交，姿态缓慢发散。每次积分后必须归一化。同理，DCM 直接积分也需要周期性做正交化（Gram-Schmidt 或 SVD）。

🔟 角度单位和归一化

• 弧度还是角度？统一在接口层转换，内部一律用弧度。
• 偏航角跨越 ±180° 时的角度差计算：直接相减会得到 350° 而不是 -10°。正确做法：

def wrap_pi(a):
    return (a + np.pi) % (2 * np.pi) - np.pi

err = wrap_pi(yaw_target - yaw_current)   # 永远走短路径

这个 bug 会让飞机在正北附近突然反向旋转一整圈，非常吓人。

---

总结

坐标系	符号	原点	主要用途
地心惯性系	i	地心	惯导力学编排的理论基础
地心地固系	e	地心	GNSS 解算、全球定位
大地坐标系	—	椭球面	位置表达（经纬高）
导航系	n	载体/起飞点	速度、位置、控制的主战场
机体系	b	重心	IMU 测量、力和力矩、控制分配
气流系	a	重心	气动力（升力/阻力）
航迹系	k	重心	轨迹规划、爬升角控制
相机系	c	光心	视觉感知

记忆主线：

$$\underset{\text{位置，靠经纬度决定}}{\underbrace{\text{LLA}\leftrightarrow \text{ECEF}\leftrightarrow \text{NED}}}\left|\underset{\text{姿态，靠欧拉角/四元数决定}}{\underbrace{\text{NED}\leftrightarrow \text{Body}}}\right|\underset{\text{安装，靠标定决定}}{\underbrace{\text{Body}\leftrightarrow \text{Camera}}}$$

三段各自的"驱动量"不同：第一段由位置驱动，第二段由姿态驱动，第三段由标定外参驱动。想清楚这一点，任何复杂的转换链路都能拆开。

最后送一条实践建议：把所有坐标转换封装成独立模块，配上完整单元测试（往返一致性、正交性、行列式为 1、重力向量检查）。这部分代码写一次可以用很多年，值得一开始就写扎实。

---

如果这篇文章对你有帮助，欢迎点赞收藏。有问题欢迎评论区讨论。