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

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

• Рассчитывает текущую 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();
}

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

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

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

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

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

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

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

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