Quadcopter: стабилизация вдоль оси z (для удержания высоты)

8

Недавно я провел некоторую работу над прошивкой моего квадрокоптера. Модель стабилизирует свое отношение относительно хорошо сейчас. Однако я заметил, что он иногда меняет свою высоту (может быть, изменение давления, ветер или турбулентность). Теперь я хочу избавиться от этих перепадов высоты и не нашел много литературы. Мой подход использует акселерометр:

  • Рассчитывает текущую g-силу оси Z
  • если сила перегрузки> 0,25 г и дольше 25 мс, я подаю термин акселерометр (см на с²) в пид
  • выход отправляется на моторы

Модель теперь реагирует, когда она падает, с повышением мощности двигателей. Тем не менее, я не уверен, разумно ли подавать текущее ускорение в регулятор, и я в настоящее время задаюсь вопросом, существует ли более разумный метод для борьбы с внезапными и меньшими изменениями высоты.

Текущий код:

# define HLD_ALTITUDE_ZGBIAS 0.25f
# define HLD_ALTITUDE_ZTBIAS 25

const float fScaleF_g2cmss = 100.f * INERT_G_CONST;
int_fast16_t iAccZOutput = 0; // Accelerometer

// Calc current g-force
bool bOK_G;
float fAccel_g = Device::get_accel_z_g(m_pHalBoard, bOK_G); // Get the acceleration in g

// Small & fast stabilization using the accelerometer
static short iLAccSign = 0; 
if(fabs(fAccel_g) >= HLD_ALTITUDE_ZGBIAS) {
  if(iLAccSign == 0) {
    iLAccSign = sign_f(fAccel_g);
  }

  // The g-force must act for a minimum time interval before the PID can be used
  uint_fast32_t iAccZTime = m_pHalBoard->m_pHAL->scheduler->millis() - m_iAccZTimer;
  if(iAccZTime < HLD_ALTITUDE_ZTBIAS) {
     return; 
  }

  // Check whether the direction of acceleration changed suddenly
  // If so: reset the timer
  short iCAccSign = sign_f(fAccel_g);
  if(iCAccSign != iLAccSign) {
    // Reset the switch if acceleration becomes normal again
    m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
    // Reset the PID integrator
    m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
    // Save last sign
    iLAccSign = iCAccSign;
    return;
  }

  // Feed the current acceleration into the PID regulator
  float fAccZ_cmss = sign_f(fAccel_g) * (fabs(fAccel_g) - HLD_ALTITUDE_ZGBIAS) * fScaleF_g2cmss;
  iAccZOutput = static_cast<int_fast16_t>(constrain_float(m_pHalBoard->get_pid(PID_ACC_RATE).get_pid(-fAccZ_cmss, 1), -250, 250) );
} else {
  // Reset the switch if acceleration becomes normal again
  m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
  // Reset the PID integrator
  m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
}
dgrat
источник
1
Как насчет использования сонара, указывающего вниз, чтобы измерить высоту?
dm76
2
Почему вы используете порог 0,4 г? Почему бы, как вы говорите, не всегда отправлять ускорение Z в PID?
Ракетный
Гидролокатор работает всего 6 м, и я хотел использовать акселерометр только для внезапных и небольших изменений. Я думал, что я должен использовать отключение, потому что акселерометр подвержен шуму датчика. Кроме того, PID должен просто работать, когда есть большие изменения, а не для каждого маленького движения.
Дггр
Провести данные акселерометра через цифровой фильтр нижних частот?
Саймон Рихтер
4
Прежде чем вы сможете удерживать высоту с какой-либо точностью, вы должны уметь измерять ее с точностью. Прежде чем вы сможете быстро реагировать, вам нужно быстро измерить . Предполагая, что вы можете мгновенно определить свою высоту (и в равной степени изменить желаемую тягу), сколько времени понадобится вашим двигателям для ускорения и сколько времени потребуется, чтобы это отразилось на массе и скорости квадрокоптера? В чем разница между той оптимальной задержкой и задержкой, которую вы видите сейчас?
Ян

Ответы:

2

Возможны два подхода:

  1. Объедините данные датчиков высоты (GPS или давления) и вертикального ускорения, чтобы рассчитать лучшую геометрическую высоту, и настройте свой вертикальный контроллер, используя эту обратную связь в вашей петле.

  2. Используйте петлю увеличения стабильности для z-ускорения (в раме кузова). В этом случае, если ваш автомобиль качается, как показано в ответе Джона, ваш автомобиль почувствует ускорение по оси z и попытается исправить это. Возможно, это не лучшая практика для работы с z-ускорением в раме кузова , так как он будет связывать крен с высотой, когда самолет катится и движется. Таким образом, можно выполнить тригонометрическое преобразование для преобразования данных a_z (в кадре тела) в a_z_inertial (в кадре инерции, например, в гравитации). Лучше всего проработать это на бумаге (у вас есть и крен, и шаг, влияющие на результат).

О текущем алгоритме:

  1. Фильтруйте ваши ускорения. Попробуйте скользящее среднее (фильтр низких частот) ваших ускорений, чтобы избавиться от шума. Например, будет нормально иметь скользящее среднее за последние 0,2 секунды.

  2. Не используйте отсечение вообще. Это делает жизнь нелинейной, и это не хорошо. Позвольте контроллеру обрабатывать все события, и пусть он реагирует на небольшие ошибки, прежде чем они станут высокими.

Гюркан Четин
источник
1

Пока не могу комментировать.

Я бы добавил гироскоп и использовал бы дополнительный фильтр или фильтр Калмана. Акселерометры в среднем правы, но не правы, прямо сейчас . Гироскопы сейчас правы , но в среднем ошибаются . Фильтр взвешивает два входа в зависимости от того, насколько они ошибочны, и выводит значение где-то между правым и правым моментом .

введите описание изображения здесь

Джон
источник
Я не вижу, как гироскоп может помочь в этом случае, поскольку он измеряет угловую скорость, а не изменение высоты. Акселерометр не дает непосредственного измерения высоты, но может обеспечить обратную связь по линейным перемещениям посредством интеграции, ну едва ...
Ugo Pattacini
Если вы обнулите гироскоп и акселерометр на своем коптере, а затем поверните его на 45 градусов по оси, гироскоп будет показывать 45, а затем начнет ошибаться. Акселерометр будет неправильным, но вскоре покажет «вниз», чтобы быть на 45. Вероятно, ваш акселерометр реагирует на нефильтрованную вибрацию. Гироскоп и фильтр будут знать, что нет никаких оснований считать показания акселерометра, и исправить это.
Jon
Аналогично, когда гироскоп сообщает, что коптер перевернут, акселерометр знает, что вниз все еще вниз.
Jon
Хорошо, вы упомянули классическую дополнительную корректировку фильтра для корректировки показаний ускорения, но я думаю, что здесь важнее использовать ускорение для получения оценки высоты. Я имею в виду, «представьте, что у нас хорошие значения ускорения, тогда нам нужно…»
Уго Паттачини,
Когда вертолет наклоняется, акселерометр-z меняется, и вертолет видит «падение», которого не происходит. Он уже заставил его работать с нулевой зоной, но хочет, чтобы это не запуталось.
Jon