崔岩的笔记——从惯性到载体：导航坐标系转换实战解析

1. 导航坐标系的基础概念

第一次接触导航坐标系时，我也被各种"系"搞得晕头转向。直到有次在调试无人机飞控时，因为坐标系搞混导致飞机"抽风"，才真正明白这些概念的重要性。导航坐标系就像我们生活中的地图，不同场景需要不同的地图视角。

最常见的惯性坐标系（i系）就像宇宙视角的GPS。想象你站在太空看地球，完全忽略地球自转和公转，这就是惯性坐标系的特点。实际应用中又细分为：

• 日心惯性坐标系：以太阳为中心，适合星际导航
• 地心惯性坐标系：以地球为中心，用于卫星导航
• 发射点惯性坐标系：以地面某点为原点，适合导弹轨迹计算

地球坐标系（e系）则像粘在地球表面的网格，会跟着地球一起转动。它的Z轴就是地球自转轴，X轴指向本初子午线。这个坐标系最贴近我们日常的经纬度概念。

2. 坐标系转换的核心原理

记得第一次实现坐标系转换时，我把方向余弦矩阵写成了九九乘法表。实际上，坐标系转换本质是找到两个坐标系之间的"共同语言"——也就是它们轴向之间的关系。

方向余弦矩阵是最基础的转换工具。比如从载体坐标系(b系)到地理坐标系(g系)的转换，可以分解为三个步骤：

1. 航向角旋转（绕Z轴）
2. 俯仰角旋转（绕X轴）
3. 横滚角旋转（绕Y轴）

用Python代码表示这个转换过程：

import numpy as np def euler_to_dcm(yaw, pitch, roll): # 航向角(Z轴)旋转 Rz = np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) # 俯仰角(X轴)旋转 Rx = np.array([ [1, 0, 0], [0, np.cos(pitch), -np.sin(pitch)], [0, np.sin(pitch), np.cos(pitch)] ]) # 横滚角(Y轴)旋转 Ry = np.array([ [np.cos(roll), 0, np.sin(roll)], [0, 1, 0], [-np.sin(roll), 0, np.cos(roll)] ]) return Rz @ Rx @ Ry # 矩阵连乘顺序很重要！

实测中发现，矩阵连乘顺序直接影响结果正确性。有次因为顺序搞反，导致无人机在横滚时反而改变了航向，这个bug让我调试了整整两天。

3. 惯性到载体的实战转换

在自动驾驶项目中，从惯性系到载体系的转换尤为关键。这相当于把"上帝视角"的导航数据，转换成车辆"眼中"的方向和位置。

具体转换流程可以分为四步：

1. 惯性系→地球系：考虑地球自转角速度
2. 地球系→地理系：加入载体经纬度信息
3. 地理系→地平系：处理当地垂线方向
4. 地平系→载体系：最终转换到车辆坐标系

每个步骤都需要考虑角速度补偿。例如地球自转角速度约为7.292115×10⁻⁵ rad/s，这个看似微小的数值，在长时间导航中会产生显著误差。

转换过程中最易出错的是角速度的叠加计算。有次我在处理载体运动引起的附加角速度时，忘记考虑科里奥利力效应，导致导航误差随时间累积。后来通过引入以下补偿项解决了问题：

ω_ie = 7.292115e-5 # 地球自转角速度 ω_en = [V_E/(R_N + h), -V_N/(R_M + h), -V_E*tan(L)/(R_N + h)] # 载体运动引起的角速度 ω_in = ω_ie + ω_en # 总角速度

4. 工程应用中的常见问题

在实际项目中，坐标系转换从来不是简单的数学运算。我曾遇到过一个典型案例：某型农业无人机在作业时，导航系统在田块边缘总是出现位置跳变。

经过排查发现问题是多坐标系混用导致的：

• 飞控使用载体坐标系(b系)
• 测绘数据使用地理坐标系(g系)
• 路径规划使用地平坐标系(t系)
• 而用户界面显示的却是地球坐标系(e系)

解决方案是建立统一的坐标系管理模块，所有输入输出都先转换到地理坐标系处理。关键代码结构如下：

class CoordTransformer: def __init__(self): self.current_pos = None # 当前位置(L,λ,h) self.attitude = None # 姿态角(yaw,pitch,roll) def update_state(self, pos, att): self.current_pos = pos self.attitude = att def i2b(self, inertial_data): # 实现惯性系到载体系的完整转换链 earth_data = self._i2e(inertial_data) geo_data = self._e2g(earth_data) body_data = self._g2b(geo_data) return body_data def _i2e(self, data): # 惯性系到地球系的具体实现 pass def _e2g(self, data): # 地球系到地理系的具体实现 pass def _g2b(self, data): # 地理系到载体系的具体实现 pass

另一个常见问题是浮点运算累积误差。长时间导航时，建议定期用绝对位置信息校正。我们在海上导航系统中，每30分钟就会用GPS信号进行一次坐标系重置。