knightsky 发表于 2014-10-23 16:53:57

MPU6050四元数解算求助

我最近在学习MPU6050姿态解算，原始数据通过四元数解算后的数据一直在正常值上下震荡，成正弦波动。我对程序中的Kp，Ki值进行调整，不管怎么调，一直是震荡的。请教大家，这是什么原因造成的？郁闷的很。

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
float accx,accy,accz;
float gyrox,gyroy,gyroz;
float axo=0,ayo=0,azo=0;//加速度计偏移量
float gxo=0,gyo=0,gzo=0;//陀螺仪偏移量
float pi=3.1415926;
float AcceRatio=16384.0;
float GyroRatio=131.0;
float Rad=180.0/pi;

//
//
#define Kp 0.025f//10.0f
#define Ki 0.0f//0.008f
#define halfT 0.001f

float q0=1,q1=0,q2=0,q3=0;
float exInt=0,eyInt=0,ezInt=0;

float Angle_roll,Angle_pitch,Angle_yaw;

void AngleUpdate(float ax,float ay,float az,float gx,float gy,float gz)
{
    float norm;
    float vx,vy,vz;
    float ex,ey,ez;
   
    if (ax*ay*az==0)
      return;
   
    norm=sqrt(ax*ax+ay*ay+az*az);
    ax=ax/norm;
    ay=ay/norm;
    az=az/norm;
   
    vx=2*(q1*q3-q0*q2);
    vy=2*(q0*q1-q3*q2);
    vz=q0*q0-q1*q1-q2*q2+q3*q3;
   
    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;
   
    gx=gx+Kp*ex+exInt;
    gy=gy+Kp*ey+eyInt;
    gz=gz+Kp*ez+ezInt;
   
    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;
   
    norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
    q0=q0/norm;
    q1=q1/norm;
    q2=q2/norm;
    q3=q3/norm;
   
    Angle_roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*Rad;
    Angle_pitch=asin(-2*q1*q3+2*q0*q2)*Rad;
    Angle_yaw=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*Rad;
}

void setup()
{
    Wire.begin();
    Serial.begin(9600);
   
    accelgyro.initialize();
   
    unsigned short times=400;
    for(int i=0;i<times;i++)
    {
      accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
      axo += ax; ayo += ay; azo += az;      //采样和
      gxo += gx; gyo += gy; gzo += gz;
    }
    axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
   
   Serial.println("Angle_roll   ,   Angle_pitch   ,   Angle_yaw");
   Serial.print(axo);Serial.print(",   ");Serial.print(gxo);
}

void loop()
{
    accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    accx=(ax-axo)/AcceRatio;
    accy=(ay-ayo)/AcceRatio;
    accz=(az-azo)/AcceRatio;
    gyrox=(ax-gxo)/GyroRatio;
    gyroy=(ay-gyo)/GyroRatio;
    gyroz=(az-gzo)/GyroRatio;
    //Serial.print(accx);Serial.print(",");Serial.print(accy);Serial.print(",");Serial.println(accz);
    AngleUpdate(accx,accy,accz,gyrox,gyroy,gyroz);
    //Kalman_Filter(Angle_roll,gyrox);
    Serial.print(Angle_roll);Serial.print(",");Serial.print(Angle_pitch);Serial.print(",");Serial.println(Angle_yaw);
    delay(100);
}

迷你强 发表于 2014-10-25 11:29:57

数学算法这块一直啃不下来。帮顶

knightsky 发表于 2014-10-25 21:25:20

一起学习，等高手帮我指导一下

五哥U五哥 发表于 2015-5-29 15:15:55

LZ解决了么，第44行那里我看了网上好多都是+，LZ那里用的是减号，我改了也没用。。。波动很有规律， 就是不能正常工作。

詹波波 发表于 2015-11-12 19:28:47

请问楼主问题解决了么

詹波波 发表于 2015-11-13 13:56:39

楼主　我知道你的程序哪里错了　那里应该是ＧＸ－ＧＸ偏移量

knightsky 发表于 2016-4-14 10:48:59

詹波波 发表于 2015-11-13 13:56 static/image/common/back.gif
楼主　我知道你的程序哪里错了　那里应该是ＧＸ－ＧＸ偏移量

竟然没发现。。。谢谢你，你调出来了吗，我放下好久，今天才又拿起来玩玩，参数不好调
页: [1]
查看完整版本: MPU6050四元数解算求助