[新手入门] 求教下各位大神有关MPU6050解算的问题,我得到的ROLL始终有...

[复制链接]
602|3
 楼主 | 2018-3-26 13:13 | 显示全部楼层 |阅读模式
本帖最后由 502650182 于 2018-3-26 13:15 编辑

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

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
 楼主 | 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
扫描二维码,随时随地手机跟帖
您需要登录后才可以回帖 登录 | 注册

本版积分规则

快速回复

您需要登录后才可以回帖
登录 | 注册
高级模式
我要创建版块 申请成为版主

论坛热帖

关闭

热门推荐上一条 /4 下一条

快速回复 返回顶部 返回列表