STM32F030FX MPU6050姿态融合算法

qinxiongxu · 发表于 2020-5-29 01:16:34

测试了网上找到的姿态融合算法代码（如下所示），调试发现q0,q1,q2,q3四元数据始终为0，运算后的pitch，roll，yaw这3个姿态角也是0比较怀疑是不是 float q0 = 1, q1 = 0, q2 = 0, q3 = 0 初始值不对？另外IMUupdate函数参数（(float gx, float gy, float gz, float ax, float ay, float az）
是直接读取的MPU6050的陀螺仪角度，加速度数据。

以下是测试的姿态融合解算代码：

#define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
float norm;

extern float pitch,roll,yaw;

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
//float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;

//先把这些用得到的值算好

float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;

//float q0q3 = q0*q3;

float q1q1 = q1*q1;
//float q1q2 = q1*q2;
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);//acc数据归一化

ax = ax/norm;
ay = ay/norm;
az = az/norm;

// estimated direction of gravity and flux (v and w)估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2);//四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;

// error is sum of cross product between reference direction of fields and direction measured by sensors
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;

// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;//将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

// integrate quaternion rate and normalise //四元素的微分方程
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;

// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

还请做过的朋友们指点指点，多谢了~