如何在C中正确整合曲线？

时间：2017-04-20 06:25:55

标签： c math

我正在从陀螺仪（MPU6050）读取数据。它给了我角速度。我试图整合这些数据，以了解陀螺仪的角度。 我这样做的方法是将读取数据乘以经过的时间并不断累加这些结果，即积分：

enter image description here

但它似乎不起作用，这是我的代码和我得到的输出：

#include <stdio.h>
#include <stdint.h>
#include <time.h>
#include <sys/types.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define A_SCALE (16384.0)
#define ANG_SCALE (131.0)

int main(int argc, char *argv[])
{
    int fd;
    int data;
    int i=0;

    float gyroXangle=0;
    float gyroYangle=0;
    float gyroZangle=0;
    float gyroOffset = 151;
    float gyroScale = 0.02;

    struct timeval startTime, stopTime;
    long timeDifference;
    int i=0;

    wiringPiSetup();
    fd = wiringPiI2CSetup(0x68);
    wiringPiI2CWriteReg8(fd, 0x6b, 0);

    if(fd==-1)
    {
        printf("can't setup device\n");
        return -1;
    }
    else
    {

        printf("successfully setup device\n");

        while(1) {
            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+8);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+9);
            short gyroX = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+10);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+11);
            short gyroY = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+12);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+13);
            short gyroZ = msb << 8 | lsb;

           //to know elapsed time between two successive readings
            if(i%2==0) {
                gettimeofday(&startTime, NULL);
            }
            else {
                gettimeofday(&stopTime, NULL);
            }

            if(i>=1)
            {
                timeDifference = abs((int)(stopTime.tv_sec - startTime.tv_sec)*1000000 + (stopTime.tv_usec - startTime.tv_usec));
                    printf("timeDifference: %d\n", timeDifference);
            }

            i++;
            gyroXangle = ((gyroX/ANG_SCALE) * timeDifference);
            printf("gyro x: %f x angle: %f \n", gyroX/ANG_SCALE, gyroXangle);
            sleep(1);
        }
    } 

    return 0;
}
当陀螺仪刚放在桌子上时，相应的输出：

gyro x: -0.442748 x angle: -0.000000 
timeDifference: 1006761
gyro x: -0.389313 x angle: -391945.125000 
timeDifference: 1006744
gyro x: -0.389313 x angle: -391938.500000 
timeDifference: 1006755
gyro x: -0.419847 x angle: -422683.406250 
timeDifference: 1006731
gyro x: -0.351145 x angle: -353508.593750 
timeDifference: 1006861
gyro x: -0.267176 x angle: -269008.656250 
timeDifference: 1006716
gyro x: -0.343511 x angle: -345818.468750

有人可以解释一下我的错误做法吗？

修改 

updated code: 
#include <stdio.h>
#include <stdint.h>
#include <time.h>
#include <sys/types.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define MPU6050_REG_DATA_START 0x3b
#define A_SCALE (16384.0)
#define ANG_SCALE (131.0)

int main(int argc, char *argv[])
{
    int fd;
    int data;
    int i=0;

    float gyroXangle=0;
    float gyroYangle=0;
    float gyroZangle=0;
    float gyroOffset = 151;
    float gyroScale = 0.02;

    struct timeval startTime, stopTime;
    double timeDifference;

    wiringPiSetup();
    fd = wiringPiI2CSetup(0x68);
    wiringPiI2CWriteReg8(fd, 0x6b, 0);

    if(fd==-1)
    {
        printf("can't setup device\n");
        return -1;
    }
    else
    {
        printf("successfully setup device\n");

        while(1) {
            //start_time = gettime_now.tv_nsec;

            uint8_t msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+8);
            uint8_t lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+9);
            short gyroX = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+10);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+11);
            short gyroY = msb << 8 | lsb;

            msb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+12);
            lsb = wiringPiI2CReadReg8(fd, MPU6050_REG_DATA_START+13);
            short gyroZ = msb << 8 | lsb;

            if(i%2==0){
                gettimeofday(&startTime, NULL);
            }
            else{
                gettimeofday(&stopTime, NULL);
            }

            if(i>=1)
            {
                timeDifference = abs((stopTime.tv_sec - startTime.tv_sec)+ (stopTime.tv_usec - startTime.tv_usec)/10.0e6);
                    printf("timeDifference: %d\n", timeDifference);
            }

            i++;
            gyroXangle += ((gyroX/ANG_SCALE) * timeDifference);
            printf("gyro x: %f x angle: %f \n", gyroX/ANG_SCALE, gyroXangle);
            //sleep(1);
        }
    } 

    return 0;
}

相应的输出：

gyro x: -0.442748 x angle: 0
timeDifference: 0
gyro x: -0.389313 x angle: 0
timeDifference: 0
gyro x: -0.389313 x angle: 0
timeDifference: 0
gyro x: -0.419847 x angle: 0
timeDifference: 0
gyro x: -0.351145 x angle: 0
timeDifference: 0
gyro x: -0.267176 x angle: 0
timeDifference: 0
gyro x: -0.343511 x angle: 0

2 个答案:

答案 0 :(得分：2)

代码至少存在一些问题。

  1. 代码在第一次分配之前使用timeDifference。这导致了UB。

        if(i>=1) {
        timeDifference = ...
    }
    i++;
    //  First time through the loop  not yet defined/assigned
    //                                vvvvvvvvvvvvvv  
    gyroXangle = ((gyroX/ANG_SCALE) * timeDifference);

  2. 错误地使用int abs(int)代替double fabs(double)和错误的常量10.e6。 @pingul 

    // timeDifference = abs((stopTime.tv_sec - startTime.tv_sec) + 
//    (stopTime.tv_usec - startTime.tv_usec)/10.0e6);
timeDifference = fabs((stopTime.tv_sec - startTime.tv_sec) + 
    (stopTime.tv_usec - startTime.tv_usec)/1.0e6);

  3. 编辑版本中的打印说明符错误。这也意味着OP没有使用完全启用警告的编译器。最好使他们能够节省调试时间。

    double timeDifference;
...
// printf("timeDifference: %d\n", timeDifference);
printf("timeDifference: %e\n", timeDifference);

  4. 微妙偏见的候选源：首先，我是一个不自信的代码将2字节输入转换为short gyroX正确给定的字节序问题@Lundin，但让我们假设它是正确的。发布许多样本gyroX会很有用。问题是A / D转换偏差。根据A / D的配置方式，读数可能代表最接近的可能读数，但 floored 读数的平均偏差为+1/2最低有效位。 #define A_SCALE (16384.0)意味着转换为14位，因此读数在2 14 中偏差为0.5。 OP可能希望微调BIAS，以免不断地整合读数的偏差。 

    #define BIAS (0.5 * 65536.0 / A_SCALE)
gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * timeDifference);

  5. 我建议将double与gyro*angle一起使用，因为这些变量持有集成计算，对累积错误很敏感。

    6. 可以简化代码的时差计算。请注意，第一个速度测量值被忽略 - 因为应该注意问题＃1 

        struct timeval then = {0};
    bool first_time = true;
    while(1) {
      // sample data
      short gyroX  = ...  
      short gyroY  = ...  
      short gyroZ  = ...  

      struct timeval now;
      gettimeofday(&now, NULL);
      long long delta_usec = (now.tv_sec - then.tv_sec)*1000000LL + 
          (now.tv_usec - then.tv_usec);
      then = now;

      if (first_time) delta_usec = 0;
      first_time = false;    
      printf("Time Difference: %lld us\n", delta_usec);

      gyroXangle += (((gyroX + BIAS)/ANG_SCALE) * delta_usec/1.0e6);
      printf("gyro x:%e, x angle:%e\n", gyroX/ANG_SCALE, gyroXangle);

答案 1 :(得分：0)

chux的答案已经涵盖了数字和编码/编码问题，因此无需再次提及它们，但我只需要添加：

我在OP中没有看到任何坐标系定义。我假设您的陀螺仪在某些移动设备上（可转动而非静态），所以您也需要考虑到这一点。我希望陀螺仪在其局部坐标系中返回值，否则它将需要一些基于不同传感器数据的参考帧，这是我在任何地方都看不到的。那该怎么做：

  1. 添加本地坐标系表示

    理想的是变换矩阵或基础向量（reper）以获取更多信息，请参阅：

  2. <强>初始化

    您需要初始化/校准应用的某个起点。例如，将传感器放置到指定的方向，然后按某个按钮/键告诉程序重置。此时，初始化您的局部坐标系矩阵（例如，到单位矩阵）。这可以一次或一次完成，以提高长热基础上的测量精度。

  3. 在本地坐标系中实施集成

    所以不要整合

    gyroXangle += ((gyroX/ANG_SCALE) * timeDifference);

    旋转坐标系矩阵。然后，您只需从结果矩阵前向基矢量计算欧拉角。

    4. 您可能希望查看相关的质量检查：

相关问题
最新问题