集成陀螺仪和加速度计读数以直线移动机器人

Question

现在，我正在使用编码器让我的机器人直线移动，但它只工作了 75%。为了更准确，我决定选择陀螺仪和加速度计读数。

目前正在使用 MPU 6050 加速度计和陀螺仪来获取设备的偏航、俯仰和滚动、加速度 x、y、z，但不知道如何使用该信息来调整直线运动的速度？

我也怀疑读数（陀螺仪，加速度计）是否正确？

dmpmpu6050.cpp

    float DmpMPU6050_Demo::Loop_Yaw()
{
    if (!dmpReady)
    {
        return 1;
    }
    fifoCount = mpu.getFIFOCount();

    if (fifoCount == 1024)
    {
        mpu.resetFIFO();
        printf("FIFO overflow!\n");
    }

    else if (fifoCount >= 42 )
    {
        mpu.getFIFOBytes(fifoBuffer, packetSize);

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        return ( ypr[0] * 180/M_PI);
        #endif

        printf("\n");
    }
}

俯仰和横滚相似。

float DmpMPU6050_Demo::Loop_Accelx()
{
    if (!dmpReady)     
    {
        return 1;
    }
    fifoCount = mpu.getFIFOCount();

    if (fifoCount == 1024)
    {
        mpu.resetFIFO();
        printf("FIFO overflow!\n");
    }

    else if (fifoCount >= 42 )
    {
        mpu.getFIFOBytes(fifoBuffer, packetSize);

        #ifdef OUTPUT_READABLE_REALACCEL
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);

        return 1;
        #endif

        printf("\n");
    }
}

加速度计 y 和 z 相似

Gyroscopethread.cpp

int numbercount=0;
float yawdata;
float accelX;

void GyroScopeThread::run()
{
    gscope = new DmpMPU6050_Demo();
    accelerometer = new DmpMPU6050_Demo();

    gscope->Setup();
    accelerometer->Setup();
    usleep(10000);

    int number = 100;    
    while (true)
    {
        if (this->gyrostop) break;

        yawdata = gscope->Loop_Yaw();

        for(int i = 0; i<2; i++)
        {
            float yawdata1 = gscope->Loop_Yaw();

            yawdata = yawdata + yawdata1;

            delay(1);
        }

        yawdata = yawdata/3;

        if(numbercount == number){

        emit Yaw_Data(yawdata);
        cout<<"yaw :"<<yawdata<<endl;

similar calculation for accelero meter 

        numbercount = 0;// count value of data
      }

    }
        numbercount = numbercount+1; // data count increment
 }

    delete gscope;
    delete accelerometer;
}

输出：

yaw :-14.9574  pitch :-18.3952  roll :-18.3952
Accelx :1.33333  Accely :1  Accelz :1
yaw :-5.5584  pitch :-5.5584  roll :-6.57062
Accelx :0.333333  Accely :0.666667  Accelz :0.666667
yaw :-11.8345  pitch :-10.9161  roll :-10.9161
Accelx :0.666667  Accely :1  Accelz :1
yaw :-4.5936  pitch :-4.5936  roll :-4.5936
Accelx :1.33333  Accely :1.33333  Accelz :1.33333
yaw :-9.574  pitch :-9.574  roll :-9.574
Accelx :0.666667  Accely :1  Accelz :1
yaw :-10.1267  pitch :-10.1267  roll :-10.1267
Accelx :1.33333  Accely :1.33333  Accelz :1.33333

当时右90:

yaw :-12.2344  pitch :-11.8448  roll :-11.8448
Accelx :1.33333  Accely :1.33333  Accelz :1.33333
yaw :0.726291  pitch :-4.36679  roll :-4.36679
Accelx :1.33333  Accely :1.33333  Accelz :1.33333
yaw :7.62387  pitch :7.62387  roll :7.62387
Accelx :1  Accely :1  Accelz :1
yaw :18.6464  pitch :18.6464  roll :18.6464
Accelx :1.33333  Accely :1.33333  Accelz :1
yaw :-4.06193  pitch :-8.62676  roll :-7.67034
Accelx :1  Accely :1  Accelz :1
yaw :-18.9466  pitch :-17.4917  roll :-12.0176
Accelx :1  Accely :1  Accelz :1
yaw :-4.94824  pitch :-9.12684  roll :-9.12684
Accelx :1  Accely :1  Accelz :1
yaw :-6.94877  pitch :-10.4829  roll :-10.4829
Accelx :1  Accely :1  Accelz :1
yaw :-19.0769  pitch :-17.6077  roll :-12.0728
Accelx :1  Accely :1  Accelz :1
yaw :-3.13396  pitch :-11.7479  roll :-10.2981
Accelx :1  Accely :1  Accelz :1
yaw :12.7717  pitch :1.98726  roll :1.98726
Accelx :0.333336  Accely :0.666668  Accelz :0.666668
yaw :-6.66976  pitch :-6.66976  roll :-6.66976
Accelx :1  Accely :1  Accelz :1

正在退出 RightMotion 90

再次直线运动：

yaw :-14.1805  pitch :-14.1805  roll :-10.3879
Accelx :0.333508  Accely :0.333508  Accelz :0.666783

Answer 1

陀螺仪 returns 表示机器人偏离其初始航向的正度数或负度数的值。只要机器人继续直行，航向将为零。例如

    class GyroSample : public SampleRobot
{
 RobotDrive myRobot; // robot drive system
 AnalogGyro gyro;
 static const float kP = 0.03;

public:
 GyroSample():
  myRobot(1, 2), // initialize the sensors in initilization list
  gyro(1)
 {
  myRobot.SetExpiration(0.1);
 }

 void Autonomous()
 {
  gyro.Reset();
  while (IsAutonomous())
  {
   float angle = gyro.GetAngle(); // get heading
   myRobot.Drive(-1.0, -angle * kP); // turn to correct heading
   Wait(0.004);
  }
  myRobot.Drive(0.0, 0.0); // stop robot
 }
};

集成陀螺仪和加速度计读数以直线移动机器人

Integrating Gyro and Accelerometer Readings to move Robot Straight

c++

robotics

accelerometer

gyroscope

raspberry-pi2