求教下各位大神有关MPU6050解算的问题，我得到的ROLL始终有...
本帖最后由 502650182 于 2018-3-26 13:15 编辑求教下各位大神，我在用MPU6050解算姿态的时候，得到的ROLL始终有问题。通过匿名上位机来观察，平放静止时，得到的ROLL始终在180度左右，左右动一下，就在+180度左右或者-180度左右变化下，飞机是倒过来的，除非将MPU6050翻转过来，那么得到的飞机是正过来的，ROLL也正常了。如果初始化的时候，就将MPU6050倒过来方，那么又会出现上面的问题。 file:///C:/Users/Administrator/AppData/Roaming/Tencent/QQ/Temp/%5D%25%5BKB0~9G 这是我姿态解算的代码：
/*************************************************************
//姿态解算
*************************************************************/
void IMUupdata(float gx,float gy,float gz,float ax,float ay,float az)
{
static u16 LastTime = 0;
float vx,vy,vz;
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;
//加速度计数据归一化成单位矢量
norm1 = MySqrt(ax * ax + ay * ay + az * az);
ax = ax / norm1;
ay = ay / norm1;
az = az / norm1;
//计算机体重力向量
//这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
//根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
//所以这里的vx、vy、vz，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
//叉乘得到误差
//ax、ay、az是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
//ax、ay、az是测量得到的重力向量，vx、vy、vz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
//那它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
//向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，ex、ey、ez就是两个重力向量的叉积。
//这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
//对误差进行积分
exInt = exInt + ex * Ki * 2 * halfT;
eyInt = eyInt + ey * Ki * 2 * halfT;
ezInt = ezInt + ez * Ki * 2 * halfT;
// //对误差进行积分
// exInt = exInt + ex * Ki;
// eyInt = eyInt + ey * Ki;
// ezInt = ezInt + ez * Ki;
//误差限幅
exInt = LIMIT( exInt,- IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );
eyInt = LIMIT( eyInt,- IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );
ezInt = LIMIT( ezInt,- IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );
//校正陀螺仪测量值
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
//用定时器6计算时间
if(TIM6->CNT > LastTime)
{
halfT = (short)(TIM6->CNT - LastTime)/2000000.0;
}
else
{
halfT = (short)(TIM6->CNT + 60000 - LastTime)/2000000.0;
}
LastTime = TIM6->CNT;
//四元数迭代
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;
//四元数归一化
norm2 = MySqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if(norm2 == 0)
{
return;
}
q0 = q0 / norm2;
q1 = q1 / norm2;
q2 = q2 / norm2;
q3 = q3 / norm2;
//归一化处理
angle.roll = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1);
angle.pitch = asin(-2*q1*q3 + 2*q0*q2);
angle.yaw = -atan2(-2*(q1*q2 + q0*q3),-2*(q0*q0 + q1*q1) + 1);
//四元数输出欧拉角
Angle_Absolute= angle.roll*RtA; //roll RtA：57.324841f //180/3.1415角度制 转化为弧度制
Angle_Absolute= angle.pitch*RtA; //pitch
Angle_Absolute= angle.yaw*RtA; //yaw
}
问题解决了，是由于MPU6050得到的陀螺仪和加速度传感器的原始数据方向没有统一，才导致解算出来的欧拉角有问题。file:///C:/Users/Administrator/Desktop/%7B%60F5@2FVKY%60Y6%5D%25%609FKI$%7BH.png 大佬 能加个QQ好友吗 我最近也在做姿态解算 但解算出来的姿态角一直在鬼畜 算法跟你一样 所以想请教一下这些问题
页:
[1]