Интеграция показаний гироскопа и акселерометра для прямого перемещения робота

Сейчас я использую кодировщик для прямого перемещения своего робота, но он работает только на 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");
}
}

Аналог Акселерометр у и г

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