Quadrotor: Estabilização ao longo do eixo z (para manter a altitude)

Recentemente, passei algum trabalho no meu firmware quadcopter. O modelo está estabilizando sua atitude relativamente bem agora. No entanto, notei que às vezes está mudando de altitude (talvez mudanças de pressão, vento ou turbulência). Agora, quero me livrar dessas quedas de altitude e não encontrei muita literatura. Minha abordagem é usar o acelerômetro:

• Calcula a força g atual do eixo z
• se a força-g for> 0,25 ge for maior que 25 ms, alimentarei o termo acelerômetro (cm por s²) no pid
• a saída é enviada para os motores

O modelo agora reage quando está caindo com uma regulação positiva dos motores. No entanto, não tenho certeza se é inteligente alimentar a aceleração atual no regulador e atualmente me pergunto se existe um método mais inteligente para lidar com mudanças repentinas e menores na altitude.

Código atual:

# 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();
}

São possíveis duas abordagens:

1. Combine os dados dos sensores de altitude (GPS ou pressão) e de aceleração vertical para calcular uma altitude geométrica melhor e ajuste seu controlador vertical usando esse feedback em seu loop.

2. Empregue um loop de aumento de estabilidade para a aceleração z (na estrutura da carroceria). Nesse caso, se o veículo girar, conforme mostrado na resposta de Jon, ele sentirá uma aceleração z e tentará corrigir isso. Pode não ser uma prática recomendada trabalhar com a aceleração z na estrutura da carroceria , pois associará a rolagem à altitude à medida que a aeronave rola e se move. Portanto, uma conversão trigonométrica pode ser feita para converter os dados a_z (na estrutura do corpo) em a_z_inertial (na estrutura inercial, por exemplo, na gravidade). É melhor trabalhar isso no papel (você tem rolo e inclinação, afetando o resultado).

Sobre o algoritmo atual:

1. Filtre suas acelerações. Tente uma média contínua (filtro passa-baixo) de suas acelerações, para se livrar do barulho. Provavelmente, não há problema em ter uma média de execução dos últimos 0,2 segundos, por exemplo.

2. Não use nenhum tipo de corte. Torna a vida não linear e não é boa. Deixe o controlador manipular todos os eventos e reagir aos pequenos erros antes que eles cresçam alto.

Ainda não é possível comentar.

Eu adicionaria um giroscópio e usaria um filtro complementar ou Kalman. Os acelerômetros estão certos, em média , mas errados, no momento . Os giroscópios estão certos agora , mas estão errados, em média . O filtro pondera as duas entradas com base em quão erradas elas são e gera um valor entre o certo e o agora .