STM32 C项目：俯仰角接近90度时四元数状态估计值异常

问题现象 #

Matlab仿真阶段，采用角轴法更新四元数的状态估计模型运行正常；但实际连接IMU硬件后，当俯仰角接近90度时，横滚角和偏航角的数值会出现异常增大的情况。

问题分析 #

取俯仰角接近90度时采集到的一个四元数 [0.705, 0.00182, 0.706, 0.0013]，通过Matlab的quat2angle函数转换后得到如下结果：

yaw = 1.8806
pitch = 1.5662
roll = 1.8813

可见横滚角和偏航角数值明显异常偏大，根源在于四元数的qx和qz分量因IMU硬件精度、计算误差等因素未能完全归零（若qx、qz严格为0则不会出现该问题）。目前已尝试引入扩展卡尔曼滤波，但未解决该异常；且该问题仅在俯仰角接近90度时触发，横滚角接近极限时状态正常，偏航角的极限情况尚未测试。

纯四元数积分代码 #

void mpu6050_PureQuaternionAxisAngle(mpu* kal, quatty* quat)
{
float T = (KALMAN_PREDICT_TIME)/1000.0f;

if ((HAL_GetTick() - timeP) >= KALMAN_PREDICT_TIME)
{
quat->PitchEstimate = asinf(2.0f*(quat->quat_y*quat->quat_s - quat->quat_x*quat->quat_z));
quat->RollEstimate = atan2f(2.0f*(quat->quat_y*quat->quat_z + quat->quat_x*quat->quat_s), (quat->quat_s*quat->quat_s - quat->quat_x*quat->quat_x - quat->quat_y*quat->quat_y + quat->quat_z*quat->quat_z));

printf("qs: %f \n ",quat->quat_s);
printf("qx: %f \n", quat->quat_x);
printf("qy: %f \n", quat->quat_y);
printf("qz: %f \n", quat->quat_z);

printf("Pitch Angle is %f \n ", quat->PitchEstimate*RAD_TO_DEGREES);
printf("Roll Angle is %f \n ", quat->RollEstimate*RAD_TO_DEGREES);


mpu6050_GyrRead_Struct(kal);

/* vector amount gyro */
float w = sqrtf(kal->GyroM[0]*kal->GyroM[0] + kal->GyroM[1]*kal->GyroM[1] + kal->GyroM[2]*kal->GyroM[2]);

if (w == 0)
{
    w = 0.01f;
}

kal->GyroM[0] = kal->GyroM[0]/w;
kal->GyroM[1] = kal->GyroM[1]/w;
kal->GyroM[2] = kal->GyroM[2]/w;

float angle = T*w;

float q_delt_s = cosf(angle/2.0f);
float q_delt_x = kal->GyroM[0]*sinf(angle/2.0f);
float q_delt_y = kal->GyroM[1]*sinf(angle/2.0f);
float q_delt_z = kal->GyroM[2]*sinf(angle/2.0f);


quat->quat_s = quat->quat_s*q_delt_s - quat->quat_x*q_delt_x - quat->quat_y*q_delt_y - quat->quat_z*q_delt_z;
quat->quat_x = quat->quat_s*q_delt_x + quat->quat_x*q_delt_s + quat->quat_y*q_delt_z - quat->quat_z*q_delt_y;
quat->quat_y = quat->quat_s*q_delt_y - quat->quat_x*q_delt_z + quat->quat_y*q_delt_s + quat->quat_z*q_delt_x;
quat->quat_z = quat->quat_s*q_delt_z + quat->quat_x*q_delt_y - quat->quat_y*q_delt_x + quat->quat_z*q_delt_s;


float norm = sqrtf(quat->quat_s*quat->quat_s + quat->quat_x*quat->quat_x + quat->quat_y*quat->quat_y + quat->quat_z*quat->quat_z);

/* Norming the quaternion */
quat->quat_s = quat->quat_s/norm;
quat->quat_x = quat->quat_x/norm;
quat->quat_y = quat->quat_y/norm;
quat->quat_z = quat->quat_z/norm;


timeP += KALMAN_PREDICT_TIME;
}
}

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