Quadcopter: estabilización a lo largo del eje z (para mantener la altitud)

8

Recientemente pasé algo de trabajo en mi firmware de quadcopter. El modelo está estabilizando su actitud relativamente bien ahora. Sin embargo, noté que a veces está cambiando su altitud (tal vez cambios de presión, viento o turbulencia). Ahora quiero deshacerme de estas caídas de altitud y no encontré mucha literatura. Mi enfoque es usar el acelerómetro:

  • Calcula la fuerza g actual del eje z
  • si la fuerza g es> 0.25 gy más de 25 ms, entonces introduzco el término del acelerómetro (cm por s²) en el pid
  • la salida se envía a los motores

El modelo ahora reacciona cuando se cae con una regulación ascendente de los motores. Sin embargo, no estoy seguro de si es inteligente alimentar la aceleración actual en el regulador y actualmente me pregunto si existe un método más inteligente para hacer frente a cambios repentinos y menores de altitud.

Código actual:

# 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
fuente
1
¿Qué tal usar un sonar apuntando hacia abajo para medir la altitud?
dm76
2
¿Por qué usas un umbral de 0,4 g? ¿Por qué no, como usted dice, siempre envía la aceleración Z al PID?
Rocketmagnet
El sonar funciona solo durante 6 my quería usar el acelerómetro solo para cambios repentinos y pequeños. Pensé que debería usar un punto de corte, porque el acelerómetro es propenso al ruido del sensor. Además, el PID debería funcionar cuando hay un cambio mayor, no para cada pequeño movimiento.
dgrat
¿Ejecutar los datos del acelerómetro a través de un filtro digital de paso bajo?
Simon Richter
44
Antes de poder mantener una altitud con alguna precisión, debe poder medir su altitud con precisión. Antes de que pueda reaccionar rápidamente, debe medir rápidamente. Suponiendo que puede determinar instantáneamente su altitud (y alterar su empuje deseado de la misma manera), ¿cuánto tiempo tardarán sus motores en acelerar y cuánto tiempo tomará para que eso tenga un efecto en la masa y la velocidad del quadcopter? ¿Cuál es la diferencia entre esa latencia óptima y la latencia que ves ahora?
Ian

Respuestas:

2

Son posibles dos enfoques:

  1. Combine los datos de los sensores de altitud (GPS o presión) y de aceleración vertical para calcular una mejor altitud geométrica, y sintonice su controlador vertical utilizando esta retroalimentación en su bucle.

  2. Emplee un bucle de aumento de estabilidad para la aceleración z (en el marco del cuerpo). En este caso, si su vehículo se balancea, como se muestra en la respuesta de Jon, su vehículo detectará una aceleración z e intentará corregirlo. Puede que esta no sea la mejor práctica para trabajar en la aceleración z en el marco del cuerpo , ya que acoplará el giro con la altitud a medida que el avión rueda y se mueve. Por lo tanto, se puede realizar una conversión trigonométrica para convertir los datos a_z (en el marco del cuerpo) en a_z_inertial (en el marco inercial, por ejemplo, en gravedad). Es mejor trabajar esto en papel (tiene tanto rollo como cabeceo, lo que afecta el resultado).

Sobre el algoritmo actual:

  1. Filtra tus aceleraciones. Pruebe con un promedio de funcionamiento (filtro de paso bajo) de sus aceleraciones, para deshacerse del ruido. Probablemente estará bien tener un promedio de los últimos 0.2 segundos, por ejemplo.

  2. No utilice el corte, en absoluto. Hace la vida no lineal, y no es buena. Deje que el controlador maneje todos los eventos, y deje que reaccione a los pequeños errores, antes de que crezcan.

Gürkan Çetin
fuente
1

No puedo comentar todavía.

Agregaría un giroscopio y usaría un filtro complementario o Kalman. Los acelerómetros tienen razón, en promedio , pero están equivocados, en este momento . Los giroscopios tienen razón, en este momento , pero están equivocados, en promedio . Los pesos del filtro de las dos entradas sobre la base de lo equivocados que están en algún lugar y generan una valor entre la derecha y en este momento .

ingrese la descripción de la imagen aquí

Jon
fuente
No veo cómo el giroscopio puede ayudar en este caso, ya que mide la velocidad angular y no la variación de altitud. El acelerómetro no proporciona una medición directa de la altitud, pero puede proporcionar información sobre los movimientos lineales a través de la integración, bueno, apenas ...
Ugo Pattacini
Si pone a cero un giroscopio y un acelerómetro en su helicóptero, luego gírelo 45 grados en un eje, el giroscopio leerá 45 en este momento, luego comenzará a estar equivocado. El acelerómetro estará mal, pero pronto mostrará "abajo" para estar en 45. Es probable que su acelerómetro esté respondiendo a la vibración sin filtrar. El giroscopio y el filtro sabrán que no hay razón para que el acelerómetro lea y corrija.
Jon
Del mismo modo, cuando el giroscopio dice que el helicóptero está al revés, el acelerómetro sabe que todavía está abajo.
Jon
Ok, has mencionado el clásico ajuste de filtro complementario para corregir las lecturas de aceleración, pero creo que aquí es más importante utilizar la aceleración para obtener la estimación de altitud. Quiero decir, "imagina que tenemos buenos valores de aceleración, entonces lo que necesitamos aquí es ..."
Ugo Pattacini
Cuando el helicóptero se inclina, el acelerómetro-z cambia y el helicóptero ve "caída" que no está sucediendo. Ya lo obligó a trabajar con la zona nula, pero quiere que deje de confundirse.
Jon