第1章导航基础与状态估计

第1章：导航基础与状态估计

各位同学，大家好。我是你们这门课的主讲。今天咱们正式开篇，聊聊导航最底层的那些东西。

说实话，我刚开始接触组合导航那会儿，也被一堆坐标系绕得头晕。什么ECEF、ENU、NED，还有各种旋转矩阵。后来做项目做多了，才慢慢摸清门道。今天我就把这些经验掰开了揉碎了讲给你听。

1.1 导航坐标系：你究竟在哪儿？

导航说白了就一件事：确定物体在哪儿，以及它要去哪儿。但“在哪儿”得有个参考系，这就是坐标系。

我个人习惯把坐标系分成两类：全局坐标系和局部坐标系。全局坐标系管地球尺度，局部坐标系管你身边那点事儿。

1.1.1 ECEF：地心地固坐标系

ECEF，全称Earth-Centered, Earth-Fixed。原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则补齐。

这个坐标系是跟着地球一起转的。你想想看，卫星导航（比如GPS）给出的原始位置，通常就是ECEF坐标。单位是米，数值很大，动辄几百万。

  关键点：ECEF是三维笛卡尔坐标系，适合做全球范围的导航解算。但它的数值变化范围大，直接用来做局部导航不太方便。

1.1.2 ENU与NED：站心坐标系

这两个是局部坐标系，原点通常设在载体或观测站上。

ENU（东-北-天）：X轴指向东，Y轴指向北，Z轴指向天顶。常用于机器人、无人机领域。
NED（北-东-地）：X轴指向北，Y轴指向东，Z轴指向地心。航空、航海领域用得更多。

我在做车载组合导航时，习惯用ENU。因为车是在地面上跑的，Z轴向上，高度变化不大，数值比较稳定。做飞机项目的同事则更偏爱NED，因为飞机俯仰、滚转的姿态定义跟NED更匹配。

我的建议：初学者先别纠结选哪个。记住一点——ENU和NED之间就差一个旋转。你只要搞懂一个，另一个自然就通了。

1.1.3 坐标系转换：绕不开的数学

坐标系之间怎么转？说白了就是旋转矩阵加平移向量。

从ECEF转到ENU，需要两步：先平移（把原点从地心移到当地），再旋转（把坐标轴对齐）。旋转矩阵由当地的经纬度决定。

// 伪代码：ECEF -> ENU 转换
// 输入：ecef_pos (x, y, z), ref_lat, ref_lon
// 输出：enu_pos (e, n, u)

// 1. 计算ECEF下的参考点坐标
ref_ecef = geodetic_to_ecef(ref_lat, ref_lon, 0)

// 2. 计算相对位置
delta = ecef_pos - ref_ecef

// 3. 构建旋转矩阵（由经纬度决定）
R = build_rotation_matrix(ref_lat, ref_lon)

// 4. 应用旋转
enu_pos = R * delta

嗯，这里要注意：旋转矩阵的构建很容易搞错符号。我当年第一次写这个函数，把经纬度搞反了，结果定位结果差了十万八千里。调试了一整天才发现。

1.2 刚体运动学：物体是怎么动的？

导航不光要知道位置，还得知道速度、姿态，以及它们怎么随时间变化。这就是运动学要干的事。

1.2.1 位置、速度、加速度

在导航里，我们通常用微分方程来描述运动：

位置对时间的导数 = 速度
速度对时间的导数 = 加速度

写成数学形式：

p_dot = v
v_dot = a

就这么简单。但实际中，加速度a通常由IMU（惯性测量单元）测量得到，里面包含重力、噪声和偏差。所以真实情况要复杂得多。

1.2.2 姿态表示：欧拉角、旋转矩阵、四元数

姿态描述的是载体坐标系相对于参考坐标系的旋转关系。常用的有三种表示方法：

表示方法	优点	缺点
欧拉角（横滚、俯仰、航向）	直观，容易理解	有万向锁问题，插值困难
旋转矩阵	无奇异，方便运算	9个参数，冗余，需正交化
四元数	无奇异，4个参数，适合插值	不够直观，需归一化

我个人在工程中几乎只用四元数。为什么？因为卡尔曼滤波里用四元数做状态量，更新起来最方便，没有万向锁的烦恼。欧拉角我一般只在最后输出给用户看的时候才用。

避坑指南：我曾经在项目里直接用欧拉角做状态估计，结果飞机做大机动时，航向角直接跳变，滤波器发散得一塌糊涂。从那以后，我再也不敢在滤波器内部用欧拉角了。

1.2.3 姿态运动学方程

四元数的运动学方程长这样：

q_dot = 0.5 * q * ω

其中q是四元数，ω是角速度（通常由陀螺仪测量）。这个方程是姿态更新的核心。你想想看，只要知道初始姿态和角速度，就能不断积分得到新姿态。

但积分会累积误差。陀螺仪有零偏，积分时间长了，姿态就会漂。这就是为什么我们需要卡尔曼滤波来修正它。

1.3 状态估计问题建模

好了，坐标系和运动学都讲完了。现在我们来回答一个核心问题：状态估计到底在做什么？

1.3.1 什么是状态？

状态，就是描述系统当前状况的一组数值。在导航里，状态通常包括：

位置（3维）
速度（3维）
姿态（3维或4维）
传感器偏差（陀螺零偏、加速度计零偏等）

加起来，一个典型的组合导航系统状态量可能有15维甚至更多。

1.3.2 状态估计的数学框架

状态估计问题可以写成两个方程：

// 状态方程（描述系统如何演化）
x_k = f(x_{k-1}, u_k) + w_k

// 观测方程（描述传感器如何测量）
z_k = h(x_k) + v_k

其中：

x_k 是k时刻的状态
u_k 是控制输入（比如IMU的加速度和角速度）
w_k 是过程噪声
z_k 是观测值（比如GPS的位置）
v_k 是观测噪声

说白了，状态方程告诉你“系统下一步会变成什么样”，观测方程告诉你“传感器看到了什么”。卡尔曼滤波就是在这两个信息之间做权衡，给出最优估计。

  核心思想：状态估计不是算命，而是融合预测和观测。预测来自模型，观测来自传感器。两者都有误差，但结合起来，误差就能被抑制。

1.3.3 一个简单的例子

假设你在一辆车上，想知道自己的位置。你有两个信息来源：

IMU：告诉你加速度，积分得到位置。但积分会漂，时间越长越不准。
GPS：直接告诉你位置。但GPS有噪声，而且可能被遮挡。

单独用任何一个都不靠谱。但如果你用卡尔曼滤波：

IMU提供高频但会漂的预测
GPS提供低频但相对准确的修正
滤波器自动决定“该信谁多一点”

这就是组合导航的精髓。

1.4 本章知识体系总览

下面这张图，是我自己画的本章知识结构。你可以把它当作一个导航地图，随时回来看看。

好了，这一章的内容就到这里。坐标系、运动学、状态估计建模，这三块是后面所有章节的基石。你如果现在觉得有点绕，没关系，后面我们会反复用到这些概念。到时候自然就熟了。

记住一句话：导航的本质，就是在不确定中寻找确定。卡尔曼滤波，就是我们手里最趁手的工具。

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

📚 课程目录