第3章导航模块核心数据结构解析

3、导航模块核心数据结构解析

做无人船导航，说白了就是跟三个东西打交道：位置、速度、姿态。这三个数据就像是船的「灵魂三问」——你在哪？跑多快？朝哪边？

我刚开始接触ArduPilot时，被里面各种坐标系搞得晕头转向。后来踩了几个坑，才慢慢理清楚。今天咱们就把这些核心数据结构掰开揉碎了讲。

3.1 位置数据结构

ArduPilot里，位置数据主要用两种方式表示：经纬度高度和NED坐标系。

经纬度高度（LLA）：这是GPS直接输出的格式。结构体长这样：

// AP_Common/Location.h
struct Location {
    int32_t lat;        // 纬度，单位：1e-7度
    int32_t lng;        // 经度，单位：1e-7度
    int32_t alt;        // 高度，单位：厘米（相对于海平面）
    uint8_t relative_alt : 1; // 是否为相对高度
    uint8_t terrain_alt : 1;  // 是否为地形高度
    // ... 其他字段
};

这里有个细节我提醒你注意：lat和lng的单位是1e-7度。什么意思？就是如果你拿到一个值372345678，实际纬度是37.2345678度。用整数存储，精度更高，计算也快。

注意：高度字段的单位是厘米，不是米！我见过有人直接拿这个值当米用，结果船在水面上飞了100米高。嗯，那画面挺美。

NED坐标系（北东地）：这是EKF内部用的坐标系。原点通常设在起飞点或某个固定参考点。结构体定义在AP_NavEKF/AP_NavEKF_core.cpp里：

// EKF内部状态向量
struct ekf_state {
    Vector3f position;    // NED位置，单位：米
    Vector3f velocity;    // NED速度，单位：米/秒
    // ... 其他状态量
};

我个人习惯把NED坐标系想象成：N是正北，E是正东，D是向下。注意D是向下为正，所以高度值通常是负数。刚开始用的时候，我老是把正负搞反，导致船一直往地底下钻。

3.2 速度数据结构

速度数据分两种：地面速度（地速）和空速（对水速度）。无人船主要用地速，因为水流影响太大，空速基本没用。

速度向量在ArduPilot里用Vector3f表示：

// AP_Math/vector3.h
class Vector3f {
    float x;  // 北向速度，单位：米/秒
    float y;  // 东向速度，单位：米/秒
    float z;  // 地向速度，单位：米/秒
};

你想想看，这个z分量在无人船上意味着什么？船在水面上，理论上z方向速度应该是0。但实际中，波浪会让船上下起伏，所以z分量会有小幅波动。我在做波浪补偿算法时，就是靠这个z分量来估算浪高的。

小技巧：如果你发现速度的z分量持续大于0.5米/秒，那大概率是GPS信号不好，或者船在「飞」。别笑，我真遇到过船被浪打飞起来的情况。

3.3 姿态数据结构

姿态数据是导航的核心。ArduPilot用两种方式表示姿态：欧拉角和四元数。

欧拉角：直观，但有万向锁问题。结构体如下：

// AP_AHRS/AP_AHRS.h
struct EulerAngles {
    float roll;   // 横滚角，单位：弧度
    float pitch;  // 俯仰角，单位：弧度
    float yaw;    // 偏航角，单位：弧度
};

四元数：没有万向锁，适合插值和滤波。EKF内部主要用四元数：

// AP_Math/quaternion.h
class Quaternion {
    float q1, q2, q3, q4;  // 四元数四个分量
};

为什么EKF要用四元数而不是欧拉角？说白了，欧拉角在俯仰角接近±90度时会「炸掉」。无人船虽然很少翻船，但万一遇到极端海况呢？我曾在一次测试中，船被浪打得横滚超过90度，如果用欧拉角，EKF直接就发散掉了。四元数就稳得很。

3.4 坐标系定义与转换

坐标系转换是导航模块里最容易出bug的地方。ArduPilot里主要涉及三个坐标系：

坐标系	缩写	用途
地球坐标系（经纬度）	LLA	GPS输出、航点存储
北东地坐标系	NED	EKF内部状态、控制律计算
机体坐标系	BODY	IMU测量、舵机控制

转换关系我用一张图来说明：

转换函数在AP_NavEKF/AP_NavEKF_core.cpp里，核心代码就几行：

// 经纬度转NED
Vector3f wgs84_to_ned(float lat, float lng, float alt, 
                      const Location& ref) {
    float dlat = (lat - ref.lat) * LAT_TO_M;
    float dlng = (lng - ref.lng) * LON_TO_M * cos(radians(ref.lat));
    float dalt = alt - ref.alt;
    return Vector3f(dlat, dlng, -dalt);  // 注意：高度取负
}

  核心要点：坐标系转换时，高度方向要取反。因为LLA里高度向上为正，NED里D向下为正。这个坑我至少踩过三次，每次都是船往水里钻才发现。

3.5 EKF状态估计器数据流

EKF（扩展卡尔曼滤波器）是导航模块的大脑。它把GPS、IMU、磁力计、气压计等传感器的数据融合在一起，输出最优的位置、速度、姿态估计。

数据流大致是这样的：

传感器数据采集：IMU以400Hz频率采集加速度和角速度，GPS以5Hz频率输出位置和速度
时间对齐：不同传感器的时间戳要统一到同一个时钟基准
预测步骤：用IMU数据推算下一时刻的状态（400Hz运行）
更新步骤：用GPS数据修正状态估计（5Hz触发）
输出：最终的位置、速度、姿态给到控制模块

EKF内部的状态向量有24个维度，但核心的就这几个：

// EKF状态向量（简化版）
struct EKF_State {
    Quaternion attitude;    // 姿态四元数
    Vector3f position;      // NED位置
    Vector3f velocity;      // NED速度
    Vector3f gyro_bias;     // 陀螺仪零偏
    Vector3f accel_bias;    // 加速度计零偏
    // ... 还有地磁、气压等
};

我曾经调试过一个诡异的问题：船在静水里漂着，但EKF输出的位置却在缓慢移动。查了两天，最后发现是陀螺仪零偏没收敛。说白了，IMU的零偏就像是一个「慢性毒药」，短时间内看不出来，时间长了就会让位置估计越偏越远。

调试技巧：在Mission Planner里打开EKF状态窗口，观察gyro_bias和accel_bias的收敛情况。如果它们一直在跳变，说明IMU可能有问题，或者震动太大。

EKF的数据流可以用下面这张图概括：

EKF的代码实现里，最核心的是update()和correct()两个函数。我建议你重点看AP_NavEKF_core.cpp里的UpdateFilter()函数，里面包含了完整的预测-更新循环。

避坑指南：EKF的协方差矩阵初始化很重要。如果初始协方差设得太小，EKF会「过于自信」，新传感器数据很难修正状态。如果设得太大，初期估计会剧烈抖动。我一般把位置协方差初始化为10米²，姿态协方差初始化为0.1弧度²，效果还不错。

好了，核心数据结构就讲到这里。记住一句话：位置是骨架，速度是血液，姿态是灵魂。这三者搞明白了，导航模块的底层逻辑你就掌握了八成。

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

📚 课程目录

3、导航模块核心数据结构解析

3.1 位置数据结构

3.2 速度数据结构

3.3 姿态数据结构

3.4 坐标系定义与转换

3.5 EKF状态估计器数据流