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

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

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: я знаю, что есть такой вопрос здесь но это не решает мою проблему, пожалуйста, помогите мне.


c embedded stm32 stm32f4discovery mpu6050
person Zero_Cool    schedule 10.01.2018    source источник
comment
Вы знаете о atan2()?   -  person Yunnosch    schedule 10.01.2018
comment
Акселерометр правильно измеряет угол (от 90 до -90), а гироскоп нет. Гироскоп измеряется в рад/сек, поэтому, я думаю, должно быть достаточно только одного интегрирования. Но я думаю, что в моей реализации чего-то не хватает. ~ Спасибо.   -  person Zero_Cool    schedule 10.01.2018
comment
@Yunnosch - Это: atan2() хорошая ссылка достаточно, или есть версия POSIX, на которую вы бы предпочли ссылаться.   -  person ryyker    schedule 10.01.2018

Ответы (2)


arrow_upward
4
arrow_downward

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

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

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

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

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

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

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;
person Clifford    schedule 10.01.2018
comment
Тогда я сделаю следующее: 1)Возьмем частоту 50Гц. 2) Вычислять Gx_Correction каждый раз при обновлении кода. Но правильно ли реализована моя интеграция? - person Zero_Cool; 10.01.2018
comment
Да, интеграция представляет собой простую накопительную сумму. - person Clifford; 10.01.2018

arrow_upward
0
arrow_downward

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

person Zero_Cool    schedule 11.01.2018