Quadcopter：z軸に沿った安定化（高度を保持するため）

最近、クワッドコプターのファームウェアにいくつかの作業を費やしました。モデルはその姿勢を比較的よく安定させています。しかし私は気づきました、それはその高度を時々変えていることに気づきました（多分圧力変化、風または乱気流）。今、私はこれらの高度の低下を取り除きたいと思い、あまり多くの文献を見つけませんでした。私のアプローチは加速度計を使用しています：

• Z軸の現在のGフォースを計算します
• g力が0.25 gを超え、25ミリ秒を超える場合、加速度計の項（cm 2 /s²）をpidに送ります
• 出力はモーターに送られます

モデルは、モーターのアップレギュレーションにより、落下時に反応します。ただし、現在の加速度をレギュレーターにフィードするのが賢明かどうかはわかりません。現在、高度の突然の小さな変化に対処するためのよりスマートな方法があるかどうか疑問に思っています。

現在のコード：

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

2つのアプローチが可能です。

1. 高度（GPSまたは圧力）と垂直加速度センサーのデータを組み合わせて幾何学的高度を計算し、このフィードバックをループで使用して垂直コントローラーを調整します。

2. Z加速（ボディフレーム内）に安定性増大ループを採用します。この場合、ジョンの回答に示されているように、車両が揺れると、車両はZ加速度を感知し、それを修正しようとします。これは、機体が回転して動き回るときにロールと高度を連動させるため、ボディフレームの Z加速に取り組むことはベストプラクティスではない場合があります。したがって、三角変換を実行して、a_zデータ（ボディフレーム内）をa_z_inertial（慣性フレーム内、重力など）に変換できます。これは紙の上で作業するのが最善です（ロールとピッチの両方があり、結果に影響します）。

現在のアルゴリズムについて：

1. 加速度をフィルタリングします。ノイズを取り除くために、加速度の移動平均（ローパスフィルター）を試してください。たとえば、最後の0.2秒の移動平均があっても問題ないでしょう。

2. カットオフは使用しないでください。それは人生を非線形にし、それは良くありません。コントローラーがすべてのイベントを処理するようにし、イベントが高くなる前に小さなエラーに反応させます。

まだコメントできません。

私はジャイロを追加し、補完またはカルマンフィルターを使用します。加速度計は、右の平均ではなく、今、間違っています。ジャイロは現在正しいですが、平均して間違っています。フィルターは、2つの入力の誤りに基づいて重み付けを行い、適切な値と現在の値の中間の値を出力します。