姿态解算知识点1——四元数互补滤波求解欧拉角_北东地 翻滚角公式

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 = q0/norm;
  q1 = q1/norm;
  q2 = q2/norm;
  q3 = q3/norm;
}

 

5.四元数转欧拉角

我们一般这样定义,pitch为俯仰角，roll为航向角，yaw为翻滚角。

地理坐标系定义遵循右手螺旋定则，描述如下：

            

显然地理坐标系一般有两种定义方式。

方式一：可以选择x轴朝右，y轴朝前，z轴垂直向上 俗称：东北天 --- 右前上，如下图：

                 

 

$pitch = asinf(2*q0*q1 + 2*q2*q3)*57.3$

$roll = atan2f(-2*q1*q3 + 2*q0*q2, -2*q1*q1 - 2*q2*q2 + 1)* 57.3$

$yaw = -atan2f(-2*q2*q1 + 2*q0*q3, -2*q1*q1 -2*q3*q3 + 1)* 57.3$

 

方式二：也可以选择x轴朝右，y轴朝前，z轴垂直向上 俗称：北东地 --- 前右下，如下图：

                    

 

$pitch = asinf(2*q1*q3 - 2*q0*q2)*57.3$

$roll = atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3；$

$yaw = -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3$

 

本文算法选择的是x轴朝右，y轴朝前，z轴垂直向上，东北天 --- 右前上。

6.数据曲线

• 机体在沿水平面运动时俯仰角曲线效果图

红色曲线-静态角度，有明显的正态分布白噪声；

绿色曲线-融合后的角度，基本不受噪声影响，比较稳定

 

• 机体朝前向上抬起时俯仰角曲线效果图

红色曲线-静态角度，有明显的波动噪声；

绿色曲线-融合后的角度，基本不受噪声影响，而且跟随特性良好

 

7.源代码

FIR滤波源代码-C语言

欧拉角微分方程算法代码

8.参考文献

• 惯性导航——秦永元
• 惯性导航基本原理——刘保中