求教下各位大神有关MPU6050解算的问题，我得到的ROLL始终有...

只看该作者 · 2018-3-26 13:13

本帖最后由 502650182 于 2018-3-26 13:15 编辑

求教下各位大神，我在用MPU6050解算姿态的时候，得到的ROLL始终有问题。通过匿名上位机来观察，平放静止时，得到的ROLL始终在180度左右，左右动一下，就在+180度左右或者-180度左右变化下，飞机是倒过来的，除非将MPU6050翻转过来，那么得到的飞机是正过来的，ROLL也正常了。如果初始化的时候，就将MPU6050倒过来方，那么又会出现上面的问题。

只看该作者 · 2018-3-26 13:14

[img]file:///C:/Users/Administrator/AppData/Roaming/Tencent/QQ/Temp/%5D%25%5BKB0~9G[2UF7LK(PHM_AQ.png[/img]

只看该作者 · 2018-3-26 13:19

这是我姿态解算的代码：
/*************************************************************
//姿态解算
*************************************************************/
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[0]= angle.roll*RtA;       //roll RtA：57.324841f //  180/3.1415  角度制转化为弧度制
  Angle_Absolute[1]= angle.pitch*RtA;       //pitch
Angle_Absolute[2]= angle.yaw*RtA;          //yaw
}

只看该作者 · 2018-3-27 12:39

问题解决了，是由于MPU6050得到的陀螺仪和加速度传感器的原始数据方向没有统一，才导致解算出来的欧拉角有问题。file:///C:/Users/Administrator/Desktop/%7B%60F5@2FVKY%60Y6%5D%25%609FKI$%7BH.png

只看该作者 · 2018-10-19 00:13

大佬能加个QQ好友吗我最近也在做姿态解算但解算出来的姿态角一直在鬼畜算法跟你一样所以想请教一下这些问题

求教下各位大神有关MPU6050解算的问题，我得到的ROLL始终有...

相关下载

相关帖子