3. 重力矢量基础：地球重力场模型、重力加速度g的取值、重力矢量在导航系中的表示、重力矢量在载体系中的投影

聊到惯性导航，重力矢量是个绕不开的话题。说白了，加速度计测到的就是比力，而比力里最稳定、最可预测的成分就是重力。我刚开始做惯导那会儿，总觉得重力就是个常数9.8，后来发现事情没那么简单。

3.1 地球重力场模型

地球重力场，并不是一个完美的球对称场。地球本身是个椭球，质量分布也不均匀。所以不同地方的重力大小和方向都有细微差别。

我们常用的模型有两种：

• 正常重力模型：把地球近似为旋转椭球，用公式计算理论重力值。比如WGS-84椭球模型。
• 实测重力模型：比如EGM2008，用球谐函数展开到高阶，精度很高，但计算量也大。

我个人习惯，在嵌入式平台上用正常重力模型就够了。除非你做的是高精度重力匹配导航，否则没必要上高阶模型。

3.2 重力加速度g的取值

g的取值，不同地方不一样。我列个常用参考值：

嗯，这里要注意。如果你做的是消费级产品，直接用9.8问题不大。但做航空或航海惯导，必须根据纬度做补偿。我曾经在一个无人机项目里，因为没做纬度补偿，悬停时高度漂移了十几米，排查了好久才发现是g值没校准。

// 根据纬度计算重力加速度
double calc_gravity(double lat_rad) {
    double sin2 = sin(lat_rad) * sin(lat_rad);
    return 9.7803267714 * (1 + 0.00193185138639 * sin2) 
           / sqrt(1 - 0.00669437999013 * sin2);
}

3.3 重力矢量在导航系中的表示

导航系，我们通常用北东地（NED）坐标系。在这个坐标系里，重力矢量很简单：

gⁿ = [0, 0, g]^T

为什么？因为导航系的z轴指向地心，重力正好沿着z轴正方向。你想想看，在北东地坐标系里，北向和东向都没有重力分量，只有地向有。

但这里有个坑。如果你用的是东北天（ENU）坐标系，重力矢量就变成了：

gⁿ = [0, 0, -g]^T

因为ENU的z轴指向天，重力是向下的，所以是负值。我在项目中遇到过好几次，因为坐标系定义搞混，姿态解算出来全是反的。所以一开始就要明确：你到底用NED还是ENU？

// 明确坐标系定义
#define NAV_FRAME_NED  1
#define NAV_FRAME_ENU  0
// 根据定义选择重力矢量符号
#if NAV_FRAME_NED
    #define GRAVITY_VECTOR {0, 0, 9.80665}
#else
    #define GRAVITY_VECTOR {0, 0, -9.80665}
#endif

3.4 重力矢量在载体系中的投影

载体坐标系（b系）是跟着设备转的。重力矢量在载体系中的投影，其实就是把导航系里的重力矢量，通过姿态旋转矩阵转过来：

g^b = C_n^b · gⁿ

其中C_n^b是导航系到载体系的旋转矩阵。展开写就是：

g_x_b = C[0][2] * g
g_y_b = C[1][2] * g
g_z_b = C[2][2] * g

你想想看，当载体水平放置时，C_n^b是单位阵，那么g^b = [0, 0, g]，加速度计只感受到z轴有重力。当载体倾斜时，重力就会分解到x轴和y轴上。

这就是我们提取姿态角的基础。比如横滚角φ和俯仰角θ，就可以从加速度计输出里算出来：

// 从加速度计输出计算横滚角和俯仰角
roll  = atan2(-accel_y, accel_z);
pitch = atan2( accel_x, sqrt(accel_y*accel_y + accel_z*accel_z) );

这里有个前提：加速度计只测到了重力，没有其他运动加速度。如果载体在加速运动，这个公式就不准了。所以实际项目中，我们通常只在静态或准静态条件下用加速度计算姿态，动态时要用陀螺仪辅助。

3.5 本章知识体系

我把这一章的核心逻辑画了个图，方便你理解：

这一章的内容，说白了就是搞清楚重力从哪来、值是多少、在导航系里长什么样、到了载体系又变成什么。这些是后面做姿态解算的基础，绕不过去。