Как бороться с разрывом угла рыскания на 180 градусов

Question

Я работаю над управлением транспортным средством и использую датчик 9DOF (акселерометр, магнитометр и гироскоп). Для угла рыскания у меня есть проблема разрыва в пи рад. (180 град.). Я управляю автомобилем с помощью ПИД-регулятора, и когда автомобиль поворачивается более чем на 180 градусов, знак внезапно меняется (со 180 до -180), и это делает контроллер странным. Та же проблема возникнет, когда она повернется более чем на -180 градусов.

В качестве метода я использую матрицу направляющих косинусов для вычисления углов Эйлера. (рекомендуемый метод для датчика искры).

У меня вопрос, какой подход я должен использовать? Как бороться с этим разрывом в случае использования ПИД-регулятора для управления углом рыскания.

Answer 1

У меня была такая же проблема, и я делаю следующее:

#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a))

Разница в 2 угла может быть просто возвращена к -180d/+180d с такой формулой.

Answer 2

Это твоя проблема?

After desired angle changes it's sign(ie. 180->-180) 
then suddenly control input(ie. P * error) becomes bigger.

Например.

--------------------------------
|  desired  |  real  |  error  |
|-----------|--------|---------|
|    170    |   160  |   10    |
|   -170    |   160  |  -330   |
--------------------------------

Если ваша проблема такая же, как я понял, как насчет этого? Прежде чем идти, обратите внимание, что предполагается, что значения каждого угла находятся в диапазоне [-pi,pi].

error = desired - real;
if(error > 180)
   error = error - 360;
else if(error < -180)
   error = error + 360;
else
   error = error;//do nothing

Этот метод всегда выбирает направление, такое, что управляющий вход становится меньше (вы знаете, что есть два варианта направления)