h2o0312 发表于 2014-5-11 12:12:26

【求教】关于MPU6050加速度积分的一点疑问

本帖最后由 h2o0312 于 2014-5-11 20:18 编辑

   导师给了个课题,要用MPU6050和arduino做路径跟踪。在论坛里潜水了一段时间,现在能得到更新四元数、方向余弦矩阵。通过互补滤波能得到还算稳定的Pitch和Roll角。现有如下几个问题:

一、Yaw角是不是必须要磁力计辅助才能得到稳定结果?用四元数算出的Yaw漂的厉害(1-2秒就跳1度,在-180到+180之间来回跳。)

二、重力在加速度三轴的分量滤除和载体坐标系到导航坐标系的转换的思路是不是:重力先转换到载体坐标系,然后载体坐标系测得的加速度减去转换后的重力,最后将去除重力分量的加速度再转换到导航坐标系?还有,下面图片里的转换方式有没问题?

三、因为要做路径跟踪,需要对加速度二次积分到位移。尝试过直接对加速度数据一次积分,可结果分分钟z轴数据就不知道漂哪去了。各位帮忙看看是我的积分程序有问题还是需要其他处理?参考其他大神的程序都是进行互补滤波或卡尔曼滤波,我要得到稳定的加速度积分可以如何处理?

四、用于积分的dt是下面程序中这样取得吗?

本人小白问题有点多,希望各位大神能指点一二

感谢@Lance热心回复。下午反复测试程序修正了些坐标变换程序里的错误,现在发现积分结果xyz任意轴只要不是和重力轴垂直,数据都会不断变大,但是垂直于竖直方向的数据就基本没多大变化(静止时),就像测试结果中xy平面水平时,vx和vy基本没什么变化,但是vz就疯跑。。。考虑可能是重力影响,那么我的重力分量滤除方法不对咯?

void loop()
{
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData << 8) | i2cData) ;// + 2000.0
accY = ((i2cData << 8) | i2cData) ;// + 3319.84
accZ = ((i2cData << 8) | i2cData) + acc_offset_Z;// + 664.48
// tempRaw = (i2cData << 8) | i2cData;
gyroX = ((i2cData << 8) | i2cData) + gyro_offset_X;// +332.0;
gyroY = ((i2cData << 8) | i2cData) + gyro_offset_Y;// +96.0;
gyroZ = ((i2cData << 8) | i2cData) + gyro_offset_Z;// +147.0;

double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
………………





//********************************************************************
// Function:四元数更新算法
//********************************************************************

void IMUupdate(double gx, double gy, double gz, double ax, double ay, double az)
{
      double norm;
      double vx, vy, vz;
      double ex, ey, ez;
      
      norm = sqrt(ax*ax + ay*ay + az*az);
      ax = ax / norm;
      ay = ay / norm;
      az = az / norm;// acc数据归一化

      vx = 2*(q1*q3 - q0*q2);
      vy = 2*(q0*q1 + q2*q3);
      vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;// estimated direction of gravity

      ex = (ay*vz - az*vy);
      ey = (az*vx - ax*vz);
      ez = (ax*vy - ay*vx);   // error is sum of cross product between reference direction of field and direction measured by sensor

      exInt = exInt + ex * Ki;
      eyInt = eyInt + ey * Ki;
      ezInt = ezInt + ez * Ki;   // integral error scaled integral gain

      gx = gx + Kp * ex + exInt;
      gy = gy + Kp * ey + eyInt;
      gz = gz + Kp * ez + ezInt;   // adjusted gyroscope measurements

      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;   // integrate quaternion rate and normalise

      norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      q0 = q0 / norm;
      q1 = q1 / norm;
      q2 = q2 / norm;
      q3 = q3 / norm;// normalise quaternion
      
      //更新方向余弦矩阵

       double t11 = q0*q0+q1*q1-q2*q2-q3*q3;
       double t12 = 2.0*(q1*q2-q0*q3);
       double t13 = 2.0*(q1*q3+q0*q2);
       double t21 = 2.0*(q1*q2+q0*q3);
       double t22 = q0*q0-q1*q1+q2*q2-q3*q3;
       double t23 = 2.0*(q2*q3-q0*q1);
       double t31 = 2.0*(q1*q3-q0*q2);
       double t32 = 2.0*(q2*q3+q0*q1);
       double t33 = q0*q0-q1*q1-q2*q2+q3*q3;

      //求出欧拉角
         roll = asin(t32) * RAD_TO_DEG;//俯仰角,绕x轴转动
         pitch = -atan2(t31,t33) * RAD_TO_DEG;//横滚角,绕y轴转动
         yaw = atan2(t12,t22) * RAD_TO_DEG;//偏航角,绕z轴转动
   
}

//********************************************************************
//Function: 消除重力加速度影响
//********************************************************************
void Sub_g(double aa, double bb, double cc )
{
      int i, j, k;
      double s = {0,0,0};
      double a = {t11, t21, t31, t12, t22, t32, t13, t23, t33};
      double b = {0, 0, g};
      
      for (i = 0; i < 3; i++)
      {
          for (j = 0; j < 1; j++)
          {
            for (k= 0; k < 3; k++)
            {
            s += a * b;
            }
          }
      }
      for ( int l = 0; l < 3; l++)
      {
          c = s;
      }      
      aa -= c;
      bb -= c;
      cc -= c;
}

//********************************************************************
//Function: 坐标变换,acc数值由b系变换至n系
//********************************************************************
void acc_convert(double x, double y, double z)
{
      double w = {0,0,0};
      double f;
      double d = {t11, t12, t13, t21, t22, t23, t31, t32, t33};
      double e = {x, y, z};
      
      for (int i = 0; i < 3; i++)
      {
          for (int j = 0; j < 1; j++)
          {
            for (int k = 0 ; k < 3; k++)
            {
            w += d * e;
            }
          }
      }
      for ( int l = 0; l < 3; l++)
      {
          f = w;
      }            
      x = f;
      y = f;
      z = f;

}
//********************************************************************
//Function: 加速度一重积分
//V(n) = V(n-1) + 0.5 * (a(n) + a(n-1)) * dt
//********************************************************************
void acc_to_vel(int n)
{
    double result = {vel_x, vel_y, vel_z};//计算结果Vn
    double previous;//前一结果Vn-1
    double al = {init_ax/16384.0, init_ay/16384.0, init_az/16384.0};//当前结果a(n)
    double ap = {0,0,0};//前一结果a(n-1)

    while (n >= 1)
    {
      n--;
      for (int i = 0; i < 3; i++)
      {
      previous = result;
      }
      for (int j = 0; j < 3; j++)
      {
      result = previous + 0.5 * (al + ap) * dt;//V(n)=V(n-1)+0.5*(a(n)+a(n-1))*dt
      }
      for (int k = 0; k < 3; k++)
      {
      ap = al;
      }
    }
    vel_x = result;
    vel_y = result;
    vel_z = result;
    velocity = sqrt(vel_x * vel_x + vel_y * vel_y + vel_z * vel_z);

Serial.print("vx =");
Serial.print(vel_x);Serial.print("\t");
Serial.print("vy =");
Serial.print(vel_y);Serial.print("\t");
Serial.print("vz =");
Serial.print(vel_z);Serial.print("\t");
Serial.print("velocity =");
Serial.print(velocity);Serial.print("\t");
Serial.print("\t");
}


   

Lance 发表于 2014-5-11 13:40:50

本帖最后由 Lance 于 2014-5-11 13:50 编辑

楼主牛人啊。
芯片中的DMP能计算出pitch,yaw,roll角度。看你给出的程序,是自己算的?
水平放置时,数据一直在增加,是板子在动?不然不怎么正常。
积分的计算,我就没尝试过了。

h2o0312 发表于 2014-5-11 14:32:49

Lance 发表于 2014-5-11 13:40 static/image/common/back.gif
楼主牛人啊。
芯片中的DMP能计算出pitch,yaw,roll角度。看你给出的程序,是自己算的?
水平放置时,数据 ...

DMP貌似调试挺麻烦的,而且据说还有数据丢失的现象,所以放弃了。
程序主要参考Madgwick的IMUUpdate计算四元数,然后用四元数求欧拉角。测试时板子倒是水平放置没动,roll、pitch看着倒也正常,就是绕z轴的yaw一直动个不停。。。

h2o0312 发表于 2014-5-11 14:34:38

自己顶一下{:soso_e185:}

Lance 发表于 2014-5-11 14:42:55

h2o0312 发表于 2014-5-11 14:32 static/image/common/back.gif
DMP貌似调试挺麻烦的,而且据说还有数据丢失的现象,所以放弃了。
程序主要参考Madgwick的IMUUpdate计算 ...

vx, vy, vz可都是自增状态啊

h2o0312 发表于 2014-5-11 15:09:05

Lance 发表于 2014-5-11 14:42 static/image/common/back.gif
vx, vy, vz可都是自增状态啊

对啊,水平vx、vy变化倒还小点,vz疯了似的加。。。不知道是我积分方法不对还是其他什麽地方出问题了,现在唯一稳定的就是pitch 和roll角,可我要的是加速度积分啊,好忧伤= =

Lance 发表于 2014-5-11 15:48:53

h2o0312 发表于 2014-5-11 15:09 static/image/common/back.gif
对啊,水平vx、vy变化倒还小点,vz疯了似的加。。。不知道是我积分方法不对还是其他什麽地方出问题了,现 ...

用这个公式V(n) = V(n-1) + 0.5 * (a(n) + a(n-1)) * dt计算,相当于Vt=Vo+at,
double dt = (double)(micros() - timer) / 1000000; 倒是没什么问题,
得看a的计算有没有问题

h2o0312 发表于 2014-5-11 20:22:22

Lance 发表于 2014-5-11 15:48 static/image/common/back.gif
用这个公式V(n) = V(n-1) + 0.5 * (a(n) + a(n-1)) * dt计算,相当于Vt=Vo+at,
double dt = (double)(mic ...

感谢提醒!又发现了一些程序错误。加速度积分漂移感觉是重力没有滤除成功的原因

h2o0312 发表于 2014-5-12 21:21:59

大神都哪去了:dizzy:

yurenpu 发表于 2014-5-14 22:19:19

我也刚开始研究这个,跟你方向一样。有兴趣一起研究啊。已经私信给你了

glen_f 发表于 2014-5-15 20:42:52

楼主,牛人!!

dancingCode005 发表于 2014-5-16 20:56:55

不知道LZ是做什么路径跟踪,不过要用MEMS器件的加速度二次积分得到位置恐怕不太现实,没有GPS自身精度有限,不可能不漂

橙色承诺 发表于 2014-8-5 11:15:06

楼主课程做的怎么样了,我也想了解这方面的经验和算法,求分享。。。

SproutME 发表于 2014-12-30 20:26:39

不知楼主有没有搞定    我现在正在做

不学不会 发表于 2015-4-28 20:10:12

大神能给解释解释用角速度积分求出角度的用法和示例程序吗?
页: [1] 2
查看完整版本: 【求教】关于MPU6050加速度积分的一点疑问