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

本文主要是介绍姿态解算知识点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轴垂直向上俗称：东北天 --- 右前上，如下图：

轴	地理坐标系	载体坐标系	欧拉角	范围	方向
X	E	右	θ- Pitch	-90° ~ +90°	绕X轴旋转
Y	N	前	γ- Roll	-180° ~ +180°	绕Y轴旋转
Z	U	上	ψ- Yaw	-180° ~ +180°	绕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轴垂直向上俗称：北东地 --- 前右下，如下图：

轴	地理坐标系	载体坐标系	欧拉角	范围	方向
X	N	前	θ- Roll	-180° ~ +180°	绕X轴旋转
Y	E	右	γ- Pitch	-90° ~ +90°	绕Y轴旋转
Z	D	下	ψ- Yaw	-180° ~ +180°	绕Z轴旋转