Quadcopter: Stabilisasi sepanjang sumbu z (untuk menahan ketinggian)

Saya baru-baru ini menghabiskan beberapa pekerjaan pada firmware quadcopter saya. Model ini menstabilkan sikapnya sekarang dengan relatif baik. Namun saya perhatikan, bahwa itu kadang-kadang mengubah ketinggiannya (mungkin tekanan berubah, angin atau turbulensi). Sekarang saya ingin menyingkirkan tetes ketinggian ini dan tidak menemukan banyak literatur. Pendekatan saya menggunakan accelerometer:

• Menghitung gaya-g saat ini dari sumbu-z
• jika g-force> 0,25 g dan lebih panjang dari 25 ms, maka saya memasukkan istilah accelerometer (cm per s²) ke dalam pid
• output dikirim ke motor

Model sekarang bereaksi ketika jatuh dengan regulasi motor. Namun, saya tidak yakin, apakah pintar untuk memasukkan percepatan saat ini ke dalam regulator dan saat ini saya bertanya-tanya, apakah ada metode yang lebih cerdas untuk menghadapi perubahan ketinggian yang tiba-tiba dan lebih kecil.

Kode saat ini:

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

Ada dua pendekatan yang mungkin:

1. Gabungkan data ketinggian (GPS atau tekanan) dan sensor akselerasi vertikal untuk menghitung ketinggian geometris yang lebih baik, dan sesuaikan pengontrol vertikal Anda menggunakan umpan balik ini dalam loop Anda.

2. Gunakan loop augmentasi stabilitas untuk akselerasi z (dalam rangka tubuh). Dalam hal ini, jika kendaraan Anda berayun, seperti ditunjukkan pada jawaban Jon, kendaraan Anda akan merasakan akselerasi z dan akan mencoba memperbaikinya. Ini mungkin bukan praktik terbaik untuk bekerja pada z-akselerasi dalam rangka tubuh , karena ia akan memasangkan gulungan dengan ketinggian saat pesawat berguling dan bergerak. Jadi konversi trigonometri dapat dilakukan untuk mengubah data a_z (dalam bingkai tubuh) menjadi a_z_inertial (dalam bingkai Inertial, misalnya dalam gravitasi). Cara terbaik untuk mengerjakan ini di atas kertas (Anda memiliki roll dan pitch, yang mempengaruhi hasilnya).

Tentang algoritma saat ini:

1. Saring akselerasi Anda. Coba rata-rata berjalan (low pass filter) dari akselerasi Anda, untuk menghilangkan kebisingan. Mungkin akan baik-baik saja untuk memiliki rata-rata berjalan 0,2 detik terakhir, misalnya.

2. Jangan gunakan cut-off sama sekali. Itu membuat hidup menjadi tidak linier, dan itu tidak baik. Biarkan pengontrol menangani semua peristiwa, dan biarkan ia bereaksi terhadap kesalahan kecil, sebelum menjadi besar.

Belum dapat berkomentar.

Saya akan menambahkan gyro dan menggunakan filter komplementer atau Kalman. Accelerometer benar, rata-rata , tetapi salah, saat ini . Gyros benar, saat ini , tetapi rata-rata salah . Filter menimbang kedua input berdasarkan seberapa salahnya mereka dan menampilkan nilai di suatu tempat antara saat ini dan saat ini .