【TI首届低功耗设计大赛】+基于mpu6050的手势遥控器设计（续3）

bennik

【TI首届低功耗设计大赛】+基于mpu6050的手势遥控器设计（续3） [复制链接]

上周在弄明白姿态解算的原理后，参考这别人的代码写了解算的程序，但是数值波动很大不知道为什么，调节参数后略微有点改善。#define kp 1.08f
#define ki 0.001f
#define deltT_half 0.01f//0.001f


融合的算法是：
void Calculate_Angle(float ax,float ay,float az,float gx,float gy,float gz){
        static float q0 = 1,q1 = 0,q2 = 0,q3 = 0;
        static float exInt = 0,eyInt = 0,ezInt = 0;
                float unit;
                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;

                if(ax*ay*az==0) return;

                unit = sqrt(ax*ax + ay*ay + az*az);
                ax = ax/unit;
                ay = ay/unit;
                az = az/unit;

                vx = 2.0*(q1q3 - q0q2);
                vy = 2.0*(q2q3 + q0q1);
                vz = q0q0 - q1q1 - q2q2 + q3q3;

                ex = (ay*vz - az*vy);
                ey = (az*vx - ax*vz);
                ez = (ax*vy - ay*vx);

                exInt = exInt + ki * ex;
                eyInt = eyInt + ki * ey;
                ezInt = ezInt + ki * ez;

                gx = gx + kp * ex + exInt;
                gy = gy + kp * ey + eyInt;
                gz = gz + kp * ez + ezInt;

                q0 = q0 + (-q1*gx - q2*gy - q3*gz)*deltT_half;
                q1 = q1 + (q0*gx + q2*gz - q3*gy)*deltT_half;
                q2 = q2 + (q0*gy - q1*gz + q3*gx)*deltT_half;
                q3 = q3 + (q0*gz + q1*gy - q2*gx)*deltT_half;

                unit = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
                q0 = q0/unit;
                q1 = q1/unit;
                q2 = q2/unit;
                q3 = q3/unit;

                Q_Angle_X = atan2(2.0 * q2 * q3 + 2.0 * q0 * q1, -2.0 * q1 * q1 - 2.0 * q2* q2 + 1.0) * 57.3;
                Q_Angle_Y = asin(-2.0 * q1 * q3 + 2.0 * q0* q2) * 57.3;
                Q_Angle_Z = atan2(2.0 * q1 * q2 + 2.0 * q0 * q3, -2.0 * q2 * q2 - 2.0 * q3* q3 + 1.0) * 57.3;
                Q_Angle_X_last = (long int)(Q_Angle_X * 100);//roll 滚动角，绕x轴转动
                Q_Angle_Y_last = (long int)(Q_Angle_Y * 100);//pitch 俯仰角，绕y轴转动
                Q_Angle_Z_last = (long int)(Q_Angle_Z * 100);//yaw 偏航角，绕z轴转动
}


但是出来mpu6050静止的时候，姿态角总是在飘，查看数据的时候发现上传的acc_x，acc_y，acc_z的波动很大，gyro到时不大，如图：

acc_x在327到348波动

acc_y在-183到-201波动

acc_z在-16309到-16283波动。不知道是不是我的PI参数没有调节好！郁闷了一周了！实在不行就换方法了

ljj3166

楼主为啥要调pi？

bennik

ljj3166 发表于 2014-12-24 13:37
楼主为啥要调pi？

简单，其实也不叫PI调节，只是给一个权重值，gyro占0.98，acc占0.02，构成一个新的角速度。

ljj3166

bennik 发表于 2014-12-24 16:37
简单，其实也不叫PI调节，只是给一个权重值，gyro占0.98，acc占0.02，构成一个新的角速度。

额，我以为你有一个闭环的控制系统呢

bennik

ljj3166 发表于 2014-12-24 19:54
额，我以为你有一个闭环的控制系统呢

你可以认为是一个闭环系统

acrux2

ljj3166 发表于 2014-12-24 19:54
额，我以为你有一个闭环的控制系统呢

你的签名不错啊