陀螺和加速度计合成四元数再转成姿态角的算法

传感器: MPU9250

单片机  C8051F310

用spi接口得到  9250  的 6个参数   角速度XYZ   加速度分量XYZ

用上面6个 参数  计算 四元数

void IMUupdate()
{
float xdata norm;
float xdata vx, vy, vz;
float xdata ex, ey, ez;


ax = (float)ACCEL[0];
ay = (float)ACCEL[1];
az = (float)ACCEL[2];


gx = (float)GYRO[0];
gy = (float)GYRO[1];
gz = (float)GYRO[2];


gx = gx*100/65536;
gy = gy*100/65536;
gz = gz*100/65536;

// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;//单位化
ay = ay / norm;
az = az / norm;


// 估计方向的重力 
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);


// 积分误差比例积分增益
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;


// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}

用计算出的四元数   计算出姿态角    仰俯角  横滚角  航向角

void recode()
{
Q[0] = q2;
Q[1] = q3;
Q[2] = q1;
Q[3] = q0;


//==========================================================
A11 = Q[0]*Q[0] - Q[1]*Q[1] - Q[2]*Q[2] + Q[3]*Q[3];
A12 = 2*(Q[0]*Q[1] + Q[2]*Q[3]);
A13 = 2*(Q[0]*Q[2] - Q[1]*Q[3]);
A23 = 2*(Q[1]*Q[2] + Q[0]*Q[3]);
A33 = -Q[0]*Q[0] - Q[1]*Q[1] + Q[2]*Q[2] + Q[3]*Q[3];

//==========================================================
P_f_1 = atan(A12/A11);
P_f_2 = -3.14f * Sign(A12) + atan(A12/A11);


R_f_1 = atan(A23/A33);
R_f_2 = -3.14f * Sign(A23) + atan(A23/A33);


Y_f = -3.14f * Sign(A13) - asin(-A13);


//==========================================================
if(A11>0)
{
P_si = (int)(P_f_2*1800/3.14f);
}
else
{
P_si = (int)(P_f_1*1800/3.14f);
}


if(A33<0)
{
R_si = (int)(R_f_1*1800/3.14f);
}
else
{
R_si = (int)(R_f_2*1800/3.14f);
}

Y_si = (int)(Y_f*1800/3.14f);


//==========================================================
if(P_si<0)
{
P_si = 32768 - P_si;
}


if(R_si<0)
{
R_si = 32768 - R_si;
}


if(Y_f<0)
{
Y_si = 32768 - Y_si;
}

//=======================
// Pitch_out = P_si + 1800;
// Roll_out = R_si + 1800;
// Yaw_out = Y_si + 1800;


//=======================
Pitch_out = P_si;
Roll_out = R_si;
Yaw_out = Y_si;
}

可承接项目   QQ: 727341554    或者短信联系 13810175694