姿态解算知识点1——四元数互补滤波求解欧拉角

最新推荐文章于 2025-02-25 15:19:51 发布

原创

最新推荐文章于 2025-02-25 15:19:51 发布 · 4.3k 阅读

60 ·

CC 4.0 BY-SA版权

文章标签：

#算法

本文详细介绍了如何通过互补滤波算法更新四元数，并将其转换为欧拉角，用于描述飞行器或机器人在空间中的姿态。具体包括四元数数学模型、公式推导、代码实现以及四元数到欧拉角的转换过程。

1.目标

求四元数q0、q1、q2、q3；
求解飞行器、机器人的欧拉角pitch、roll、yaw；

2.算法总框图

3.四元数数学模型及公式推导

上面的b系就是机体坐标系，R系可以认为是导航坐标系(即地理坐标系n)。

4. 四元数更新代码实现

/******************************************************************************

* Function Name : update_quaternion
* Description    : 更新四元数，使用互补滤波
* Input         : ax/ay/az: IMU加速度原始采样值， gx/gy/gz：IMU陀螺仪原始采样值
* Output       : None
* Return       : None

******************************************************************************/
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;//四元数
static float exInt = 0, eyInt = 0, ezInt = 0;
static void update_quaternion(float ax, float ay, float az, float gx, float gy, float gz)
{
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;

  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q1q1 = q1*q1;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;

  if(ax*ay*az==0)
      return;

  // 第一步：对加速度数据进行归一化
  norm = sqrt(ax*ax + ay*ay + az*az);
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  // 第二步：DCM矩阵旋转
  vx = 2*(q1q3 - q0q2);
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  // 第三步：在机体坐标系下做向量叉积得到补偿数据
  ex = ay*vz - az*vy ;
  ey = az*vx - ax*vz ;
  ez = ax*vy - ay*vx ;

  // 第四步：对误差进行PI计算，补偿角速度
  exInt = exInt + ex * Ki;
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  gx = gx + Kp*ex + exInt;
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;


  // 第五步：按照四元数微分公式进行四元数更新
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

  //规范化Pitch、Roll轴四元数
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 =