2. 导航基础

惯性导航系统（INS）是组合导航的核心基础，它完全依靠自身搭载的加速度计和陀螺仪，通过对载体加速度和角速度的积分，连续推算出位置、速度和姿态信息。与卫星导航不同，INS不依赖外部信号，具有完全自主、输出频率高、短期精度高的特点，但误差会随时间累积。理解INS的数学原理、坐标系定义、姿态表示方法以及地球模型与重力场模型，是搭建多源传感器融合导航系统的第一道门槛。本章将围绕这四个核心模块展开，帮助读者建立从物理器件到数学模型的完整认知链路。

2.1 惯性导航系统（INS）核心原理

INS的基本工作原理基于牛顿力学定律。安装在载体上的三轴加速度计测量比力（Specific Force），即载体相对于惯性空间的加速度减去重力加速度；三轴陀螺仪测量载体相对于惯性空间的角速度。通过对角速度积分获得姿态，利用姿态将加速度计测量的比力从载体坐标系（b系）投影到导航坐标系（n系），再扣除重力加速度和哥氏加速度，对剩余加速度进行积分得到速度，对速度再次积分得到位置。

整个解算过程可以概括为以下三个核心步骤：

1. 姿态更新：利用陀螺仪输出的角速度，通过四元数或方向余弦矩阵微分方程更新当前姿态。姿态精度直接决定了后续所有解算的精度，是INS中最关键的一环。
2. 比力分解：将加速度计输出的比力矢量，通过当前姿态矩阵从b系转换到n系，得到导航坐标系下的加速度分量。
3. 导航计算：在n系下扣除重力加速度和哥氏加速度（由地球自转和载体运动引起），对净加速度进行积分，依次得到速度和位置。

2.2 坐标系定义与转换

多源融合导航涉及多个坐标系的定义和相互转换，这是整个系统数学建模的基础。常用的坐标系包括：

坐标系之间的转换关系是INS解算的核心数学工具。从b系到n系的转换通常使用方向余弦矩阵（DCM）C_b^n，该矩阵由姿态角（横滚角φ、俯仰角θ、航向角ψ）构成，也可以通过四元数直接构造。从n系到e系的转换涉及经纬度（L, λ），而从e系到i系的转换则与地球自转角速度ω_ie有关。

2.3 姿态表示：欧拉角与四元数

姿态表示是INS中最容易出错的部分。欧拉角直观易懂，但存在万向锁（Gimbal Lock）问题；四元数计算效率高、无奇异，但不够直观。实际工程中，两者经常需要互相转换。

欧拉角通过三个依次旋转的角度描述姿态，常见的旋转顺序为Z-Y-X（航向-俯仰-横滚）。其优点是物理意义明确，便于调试和可视化；缺点是当俯仰角接近±90°时，航向角和横滚角无法唯一确定，即出现万向锁。

四元数是一个四维超复数，表示为 q = q0 + q1·i + q2·j + q3·k，满足模长为1的约束。四元数避免了三角函数运算，仅需乘法和加法即可完成姿态更新，且不存在奇异问题，因此被广泛应用于INS实时解算中。

// 四元数姿态更新（一阶龙格-库塔法）
// 输入: q (当前四元数), omega (陀螺仪角速度, rad/s), dt (采样周期)
void quaternionUpdate(double q[4], double omega[3], double dt) {
    double dq[4];
    double norm = sqrt(omega[0]*omega[0] + omega[1]*omega[1] + omega[2]*omega[2]);
    double half_dt = 0.5 * dt;
    
    dq[0] = -half_dt * (omega[0]*q[1] + omega[1]*q[2] + omega[2]*q[3]);
    dq[1] =  half_dt * (omega[0]*q[0] + omega[2]*q[2] - omega[1]*q[3]);
    dq[2] =  half_dt * (omega[1]*q[0] - omega[2]*q[1] + omega[0]*q[3]);
    dq[3] =  half_dt * (omega[2]*q[0] + omega[1]*q[1] - omega[0]*q[2]);
    
    q[0] += dq[0]; q[1] += dq[1]; q[2] += dq[2]; q[3] += dq[3];
    
    // 归一化
    double n = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] /= n; q[1] /= n; q[2] /= n; q[3] /= n;
}

2.4 地球模型与重力场模型

INS解算中，重力模型的准确性直接影响速度和位置的积分精度。地球并非标准球体，而是近似椭球体，且质量分布不均匀，导致重力加速度的大小和方向随位置变化。

WGS-84椭球模型是GPS和INS领域广泛使用的地球模型，定义了长半轴a = 6378137.0m，扁率f = 1/298.257223563。基于该模型，可以计算任意纬度处的正常重力值（Somigliana公式）：

// WGS-84 正常重力计算 (Somigliana公式)
// 输入: lat (纬度, rad), h (椭球高, m)
// 输出: 正常重力值 (m/s²)
double wgs84_gravity(double lat, double h) {
    const double a = 6378137.0;        // 长半轴
    const double f = 1.0/298.257223563; // 扁率
    const double gm = 3986004.418e8;    // 地球引力常数 (m³/s²)
    const double omega = 7.292115e-5;   // 地球自转角速度 (rad/s)
    
    double e2 = f * (2 - f);            // 第一偏心率平方
    double sin_lat = sin(lat);
    double sin2 = sin_lat * sin_lat;
    
    // 椭球面上正常重力
    double ge = 9.7803253359;           // 赤道重力
    double gp = 9.8321849378;           // 极地重力
    double k = (gp - ge) / ge;
    double gamma0 = ge * (1 + k * sin2) / sqrt(1 - e2 * sin2);
    
    // 高度修正 (自由空气修正)
    double gamma_h = gamma0 * (1 - 2*h/a * (1 + f + sin2 - 2*f*sin2));
    
    return gamma_h;
}

在实际INS解算中，还需要考虑离心加速度的影响。当载体在地球表面运动时，地球自转产生的哥氏加速度和离心加速度必须从加速度计测量值中扣除。这些修正项与载体速度、纬度和地球自转角速度有关，通常在速度更新方程中一并处理。

2.5 案例拆解：MEMS INS 静态对准与初始姿态确定

在搭建融合导航系统时，INS的初始对准是决定后续导航精度的关键步骤。以下是一个典型的MEMS INS静态初始对准案例，展示了如何利用加速度计和磁力计确定初始姿态。

背景与约束：某地面无人车辆搭载了一款消费级MEMS IMU（加速度计量程±8g，陀螺仪量程±2000°/s），需要在静止状态下完成初始对准。车辆停放在水平地面上，但存在约2°的坡度，且周围有铁质建筑可能干扰磁力计。

初始现象：直接读取加速度计输出，发现Z轴读数约为9.78 m/s²，X和Y轴读数不为零，说明载体并非完全水平。陀螺仪输出存在约±0.5°/s的零偏波动。

分析判断过程：

1. 利用加速度计在静止状态下的测量值，通过 roll = atan2(f_y, f_z) 和 pitch = atan2(-f_x, sqrt(f_y² + f_z²)) 计算初始横滚角和俯仰角。
2. 由于车辆静止，陀螺仪输出应接近零，但实际存在零偏。取1000个采样点的平均值作为陀螺零偏估计值，在后续解算中扣除。
3. 磁力计用于确定初始航向角。但由于周围铁质建筑可能引起磁干扰，需要先进行硬磁和软磁校准。校准方法：在水平面内旋转载体360°，记录磁力计输出，拟合椭圆方程，计算偏移量和比例因子。

实际操作步骤：

// 步骤1: 采集静止数据 (100Hz, 10秒)
// 步骤2: 计算加速度计均值
double ax_mean = mean(ax_data);  // 假设得到 0.12 m/s²
double ay_mean = mean(ay_data);  // 假设得到 -0.08 m/s²
double az_mean = mean(az_data);  // 假设得到 9.78 m/s²

// 步骤3: 计算初始横滚和俯仰
double roll = atan2(ay_mean, az_mean);   // ≈ -0.0082 rad ≈ -0.47°
double pitch = atan2(-ax_mean, sqrt(ay_mean*ay_mean + az_mean*az_mean)); // ≈ 0.0123 rad ≈ 0.70°

// 步骤4: 计算陀螺零偏
double gx_bias = mean(gx_data);  // 假设得到 0.02 rad/s
double gy_bias = mean(gy_data);  // 假设得到 -0.015 rad/s
double gz_bias = mean(gz_data);  // 假设得到 0.01 rad/s

// 步骤5: 磁力计校准后计算航向
double heading = atan2(my_cal, mx_cal) - declination; // 假设得到 1.52 rad ≈ 87°

结果对比：对准完成后，将初始姿态转换为四元数，并开始导航解算。在静止状态下运行60秒，位置漂移约为2.3米，主要来源于加速度计零偏和陀螺仪零偏的残余误差。若未进行零偏扣除，位置漂移会增大到15米以上。

风险与代价：磁力计校准需要载体在水平面内旋转，如果场地受限无法完成完整旋转，航向角精度会下降。此时可考虑使用双天线GPS或视觉辅助提供航向参考。

复盘经验：初始对准的精度直接决定了导航系统的长期稳定性。对于MEMS级IMU，静态对准只能消除常值零偏，随机游走误差无法通过对准消除。在融合导航系统中，建议将初始对准结果作为卡尔曼滤波器的初始状态，并在运动过程中持续估计和修正残余误差。