MPU6050 - Измерение угла наклона гироскопа для самобалансирующегося робота?

Question

Я рассчитал угол наклона акселерометра по следующей формуле:

Angle_Accel = atan(Ax/sqrt(Ay*Ay+Az*Az))*(180/PI)

Сейчас я хочу вычислить угол наклона по гироскопу и использую координату Gx для интегрирования следующим образом, но мой результат неверен.

Код Псуедо

• Измеряйте Gx каждые 0,1 секунды.
• После коррекции коэффициента чувствительности и смещения я умножаю на 180/PI, чтобы преобразовать в ухудшение.
• Затем я делю на частоту, т.е. 10, и добавляю ее к конечному углу.

Код C

Gx = (((float)GYRO_PLAY.Gyroscope_X  )* GYRO_PLAY.Gyro_Mult)-Gx_Correction;

Gy = (((float)GYRO_PLAY.Gyroscope_Y  )*  GYRO_PLAY.Gyro_Mult)-Gy_Correction;

Gz = (((float)GYRO_PLAY.Gyroscope_Z  )* GYRO_PLAY.Gyro_Mult)-Gz_Correction;

Gx_temp = (Gx*degrees)/10.0; //degrees = 180/PI

Gx_Theta = Gx_Theta + Gx_temp;

Мой угол не верен. Как мне интегрировать? Буду признателен за любую оказанную помощь.

PS: я знаю, что здесь есть такой вопрос, но он не отвечает на мою проблему, поэтому, пожалуйста, помогите мне.

Answer 1

Сэмплирование на 10 Гц кажется слишком низким, и вы в любом случае выполняете ненужную работу с каждым сэмплом. Примените необработанное смещение смещения и интегрируйте необработанное значение - преобразование в градусы / сек, если необходимо, можно выполнить при представлении, а промежуточное преобразование в радианы / сек не имеет смысла.

Робот не заботится или даже не понимает юниты, которые вам не нужны для конвертации; просто знак и величина - с чувствительностью можно справиться вашими коэффициентами контроллера замкнутого контура.

Как Gx_Correction определено - оно будет меняться со временем с температурным дрейфом; если он неверен или не отслеживается каким-либо образом, ваш интегратор увеличит ошибку.

Обратите внимание, что более высокая частота дискретизации по сравнению с SPI может оказаться невозможной - для этого предназначен встроенный DPM.

Другим возможным источником ошибки является использование float, STM32F4 имеет FPU одинарной точности, поэтому операция будет выполняться аппаратно - однако, если вы используете с плавающей запятой в контекстах прерываний или потоков, имейте в виду, что регистры с плавающей запятой вряд ли будут сохраняться между контекстами, если вы явно не реализовали это Так, например, операция с плавающей запятой будет повреждена, если прервана прерыванием, которое выполняет операции с плавающей запятой.

Если интегратору нужно работать только со значениями необработанных данных, с плавающей запятой нет необходимости:

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;

Answer 2

Приходится интегрировать Gy вместо Gx. Кроме того, угол, измеренный гироскопом, находится в градусах / с, поэтому нет необходимости умножать на degrees, Частота интеграции также должна быть 50 Гц.