基于任意初始姿态IMU的车辆航向计算问题求助

核心思路 #

由于IMU与车辆相对固定，开机静止时的重力向量可建立车辆局部坐标系与地面坐标系的初始关联，结合实时IMU四元数，就能解算出车辆绕地面垂直轴的扭转角度（航向）。

具体步骤 #

1. 确定开机初始姿态基准 #

开机静止时，IMU加速度计输出即为IMU坐标系下的重力向量，记为 g_imu_init（三维向量，如[gx, gy, gz]）：

• 定义地面坐标系垂直轴（重力反方向）为 g_world = [0, 0, 1]（可根据实际坐标系调整）；
• 计算将g_imu_init旋转到g_world的初始姿态四元数q_init，公式如下：

  // 计算向量叉乘与点乘
  float cross_x = a.y*b.z - a.z*b.y;
  float cross_y = a.z*b.x - a.x*b.z;
  float cross_z = a.x*b.y - a.y*b.x;
  float dot = a.x*b.x + a.y*b.y + a.z*b.z;
  float s = sqrt((1 + dot) * 2);
  // 四元数格式：[w, x, y, z]
  float q_w = s / 2.0f;
  float q_x = cross_x / s;
  float q_y = cross_y / s;
  float q_z = cross_z / s;
  

  其中a为g_imu_init，b为g_world，最终得到的q_init是IMU初始姿态到地面坐标系的旋转四元数。

2. 计算车辆相对地面的实时姿态四元数 #

假设实时获取的IMU坐标系四元数为q_imu_current，则：

• 先求q_init的共轭四元数：对于四元数[w,x,y,z]，共轭为[w,-x,-y,-z]；
• 车辆相对地面的实时姿态四元数 q_vehicle_world = 共轭(q_init) * q_imu_current，四元数乘法公式：

  // q1=[w1,x1,y1,z1], q2=[w2,x2,y2,z2]
  float w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
  float x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
  float y = w1*y2 - x1*z2 + y1*w2 + z1*x2;
  float z = w1*z2 + x1*y2 - y1*x2 + z1*w2;
  

3. 从姿态四元数提取航向角 #

航向角即绕地面垂直轴的旋转角，可通过四元数直接计算偏航角（Yaw）：

// q为车辆相对地面的姿态四元数[w,x,y,z]
float yaw_rad = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));
float yaw_deg = yaw_rad * 180.0f / M_PI; // 转换为角度

得到的yaw是车辆相对开机初始朝向的扭转角度，符合无特定基准的需求。

4. 关键注意事项 #

• 开机时必须保证车辆静止，否则加速度计数据会包含运动加速度，导致初始基准错误；
• 若IMU与车辆坐标系存在固定安装偏移，需提前校准一个固定旋转四元数q_imu_vehicle，先将IMU四元数转换为车辆坐标系四元数：q_vehicle_current = q_imu_vehicle * q_imu_current，再执行上述流程。

C语言核心代码示例 #

#include <math.h>
#include <stdio.h>

typedef struct {
    float w;
    float x;
    float y;
    float z;
} Quaternion;

void vector_cross(const float a[3], const float b[3], float out[3]) {
    out[0] = a[1]*b[2] - a[2]*b[1];
    out[1] = a[2]*b[0] - a[0]*b[2];
    out[2] = a[0]*b[1] - a[1]*b[0];
}

float vector_dot(const float a[3], const float b[3]) {
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

Quaternion quat_from_two_vectors(const float a[3], const float b[3]) {
    Quaternion q;
    float cross[3];
    vector_cross(a, b, cross);
    float dot = vector_dot(a, b);
    float s = sqrt((1 + dot) * 2);
    q.w = s / 2.0f;
    q.x = cross[0] / s;
    q.y = cross[1] / s;
    q.z = cross[2] / s;
    return q;
}

Quaternion quat_conjugate(const Quaternion q) {
    Quaternion q_conj;
    q_conj.w = q.w;
    q_conj.x = -q.x;
    q_conj.y = -q.y;
    q_conj.z = -q.z;
    return q_conj;
}

Quaternion quat_multiply(const Quaternion q1, const Quaternion q2) {
    Quaternion q;
    q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
    q.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
    q.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
    q.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
    return q;
}

float quat_to_yaw(const Quaternion q) {
    return atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z));
}

int main() {
    // 示例：开机静止时IMU采集的重力向量
    float g_imu_init[3] = {0.1f, 0.2f, 9.8f};
    // 地面坐标系垂直轴（z轴向上）
    float g_world[3] = {0.0f, 0.0f, 1.0f};
    
    Quaternion q_init = quat_from_two_vectors(g_imu_init, g_world);
    // 示例：实时获取的IMU四元数
    Quaternion q_imu_current = {0.99f, 0.05f, 0.03f, 0.02f};
    
    Quaternion q_init_conj = quat_conjugate(q_init);
    Quaternion q_vehicle_world = quat_multiply(q_init_conj, q_imu_current);
    
    float yaw_rad = quat_to_yaw(q_vehicle_world);
    float yaw_deg = yaw_rad * 180.0f / M_PI;
    
    printf("车辆航向角（相对开机初始姿态）：%.2f 度\n", yaw_deg);
    return 0;
}

内容的提问来源于stack exchange，提问作者Ryan