クアドロコプターの高さを安定させる問題の例を使用して、いくつかのセンサーで慣性航法システム（ANN）を使用する

この記事では、マルチローター航空機（私の場合、クアドロコプター）の高さを安定化する問題を解決するために、ANNの一部であるいくつかの標準センサー（IMUの英語版）からの信号を処理するアルゴリズムを作成および実装した私の経験についてお話します。 ハブには、それがどのようなおもちゃで、自分で作る方法を説明した記事がすでにたくさんありました。 専門職のプログラマーとして、私はそれを収集するだけでなく、「頭脳」を掘り下げ、コミュニティに役立つ何かをすることに興味がありました。 Arduinoと素晴らしいMultiWiiプロジェクトを「頭脳」として選びました。 完全にオープンで動的に開発されていますが、これまでのところ「ホワイトスポット」があります。 たとえば、高さの位置の安定化はうまく機能しません。 そして、利用可能な機器でシステムのこの部分を改善できるかどうかを判断することにしました。

まず、対処しなければならないことを明確にするための背景情報。
マルチローターは、プロペラを備えたいくつかの（3、4、6、8）モーターを備えた装置であり、それぞれが垂直方向に調整可能なトラクションを生成します。 ヘリコプターとは異なり、ここでの安定化は完全に電子的であり、マイクロプロセッサーはANN（フライトコントローラー）を使用してそれに取り組んでいます。

飛行中にどのタスクに取り組む必要がありますか？

• 方向の決定（地面に対する3つの軸に沿った角度）およびそれらに沿った安定化
• 高さの決定とその安定化
• 特定のポイントでの座標と飛行の決定
• コントロールパネルからコマンドを受信し、モーターに制御信号を発行する

自由に使えるものは何ですか？

現時点では、簡単にアクセスできるように、標準のセンサーセットがあります。

• かなり良い3軸ジャイロスコープ。
• 平均品質の3軸加速度計
• 平均品質の3軸磁力計
• 中または低気圧

Arduinoのようなプロセッサで組み立てられたキット、または別のスカーフとして組み立てられたキットは、70〜100ドルで入手できます。

各センサーには独自の機能と欠点があります。 それとは別に、それらのどれも上記の問題のいずれも解決できないため、ANNシステムは常にセンサーの組み合わせから構築されます。ここで最も興味深いのは、各センサーの長所を組み合わせてそれらの欠点を排除できる計算アルゴリズムです。

最初のタスクである方向の安定化は、ジャイロスコープによってかなりうまく解決されています。 ジャイロスコープは角速度を非常に正確に測定し、積分後に角度を取得できます。 しかし、彼らには問題があります-証言は時間とともに消えていきます。 このドリフトを補正するために、加速度計が使用されます。加速度計は常に（または長期的にはほぼ常に）地球の位置を認識しています。 しかし、加速度計は、Z軸を中心に回転させても何も感じません。そのため、北の位置を常に把握している磁力計が必要です。
2番目のタスク-高さを見つける-は、気圧計によって部分的に解決されます。 地上の測定値をリセットした場合、すべてのメーターを登ると、彼の測定値がどれだけ変化するかがわかります（当然、12時間飛行せず、天気が変わらない場合）。 しかし、問題の状態により、気圧計は私たちにとって悪いものであり、ほぼ同じ範囲の振幅を持つ野生のノイズで+ -1mの高さを与えます。 実際には、私のセンサーは次を示しています（10秒で1メートル移動します）：



ソナーは、非常に高い精度で高さを測定する気圧計の助けを借りて来ることができます（私が5ドルで購入したものでも、メーカーの声明によると±3mmの精度が得られます）。 しかし、ソナーは地上（2〜10 m）でしか機能せず、長時間（最大200ミリ秒）の対策を講じ、表面の品質、傾斜角、および信号を失う可能性があります。
3番目のタスク-座標の決定-は、上記のセンサーではまったく解決されません。 加速度計とジャイロスコープを組み合わせて水平方向の直線加速度を生成できますが、2つの問題があります。1Gの巨大なベクトル（測定対象と比較して）が常に作用していることと、補正の補正がないことです。 したがって、座標の決定はGPSセンサーの特権であり、ここでは高精度を当てにすることはできません。

すべてのアマチュアフライトコントローラーで、方向を見つけるタスクは十分に解決されているので、ここでは詳しく説明しません。 タスクは非常にシンプルで、インターネット上で描かれています（ one 、 two ）。 MultiWiiは、小さな角度と補完フィルターの簡素化に基づいて、クォータニオンやDCMマトリックス（単純な16 MHzプロセッサーであることを忘れないでください）などの問題のない美しいソリューションを使用します。

そのため、空間内での装置の向きを高い精度で把握しています。 ここで、記事のメイントピックに移ります。つまり、バロメーター（またはソナー）が生成する結果を改善して、 PIDコントローラーに送信できるようにします 。 このため、証言は遅滞なく届き、短期的には正確であり、時間とともにあまり離れてはならない。 PIDコントローラーのトピックは、プロセス制御システムで広く使用されているため、個別の綿密な調査に値します。 以下に概説する理由をよりよく理解するために、まずその定義に慣れることをお勧めします。

スムージング

現在の形式でのバロメーターの読み取りは、私たちに適さないのは何ですか？ さて、まず、信号の強いノイズはモーターに不必要な制御効果をもたらします。 ローパスフィルターを適用すると、ノイズは減少しますが、測定速度が低下します。 これは、短期的な外乱は無視され、鋭い外乱は長い遅延で解決されることを意味します。最も重要なことは、PIDコントローラーの微分成分（D）を取得しないことです。 そして、理論から次のように、このコンポーネントのないレギュレータは、実際に観察される目標値の周りの弱い減衰振動を起こしやすいです。

統合する

では、気圧計を離れて加速度計を取りましょう。 すべてが単純なようです-Z軸に沿った値から定数1Gを引き、線形垂直加速度を取得します。 これを2回統合し（実際、測定サイクルで要約します）、速度と相対変位を取得します。 PIDコントローラーの場合、これらはちょっとしたもので、適切な動的モデルを構築できます。 しかし、ここでは、すべてが私たちが望むほど良いわけではありません。 装置の傾きにより、加速度ベクトルAのZ軸への投影が変化します。モーターからの振動や温度の変化により、感度が「シフト」し、1G定数が現実と一致しなくなります。 しかし、完全に静止したデバイスで正確に1Gに設定されている場合でも、どのセンサーでもノイズが発生します。 しかし、二重統合の10秒以内の小さなミスでさえ、ゾウのサイズに成長し、今では速度が10m / sで高さが20mになっています（地面から降りることさえしていませんが）。

相補フィルター

簡単な方法で説明すると、このフィルターは、異なるセンサーで測定された2つの量に適用され、そのうちの1つを修正して、2番目にゆっくりと傾向があります。 測定サイクルでは、フィルターは単純な式で実装されます。



この場合、A1に対するA2の効果は、それらの間の差に比例し、係数kによって決定されます（大きいほど、効果は弱くなります）。
このフィルターを加速度計と気圧計の測定値で見つけた高さに適用すると、興味深いことがわかります：加速度計のドリフトは気圧計で常に調整され、気圧計の測定値は平滑化されます（A2ではこのフィルターはローパスフィルターのように機能するため）。 ただし、2番目の積分のみが修正され、最初の積分は冷静に無限に「ドリフト」します。その結果、係数kが小さいため、気圧計は状況に影響を与えません。
このフィルターがジャイロスコープと加速度計のペアでうまく機能するのはなぜですか？ そこで、最初の積分を修正し、1サイクル中の修正値が積分中に同じサイクルで追加されたジャイロスコープエラーの値と等しくなると、最終的に「浮動」しなくなります。

PIDコントローラーが助けになります

ただし、PIDコントローラを適用すると、気圧計と加速度計のペアから有用なものを抽出することもできます（はい、その範囲は非常に広範囲です）。
それで、私たちの加速器積分器の主な弱点は何ですか？ 定数1Gを差し引くときに、上記のさまざまな理由で発生する可能性があるマイクロエラー。 目的の加速をフォームに記述する場合：



次に、バイアス値を調整することで、最初の積分（速度）と2番目の積分（変位）の両方を制御できます。 したがって、PIDコントローラーの目標が見つかりました。 しかし、あなたはまだ間違いを知る必要があります。 システムパラメータ（温度、振動など）がある程度安定した後、バイアスが固定されると仮定します。 バイアスが見つかると、加速度計の測定値は真実に非常に近くなり、それらをバロメーターと交差させることで補完的なフィルターを適用できます。 このフィルターの補正値は、PIDコントローラーが反発するエラーになります（ターゲット変数を制御することにより、エラーを0に減らします）。
次に、PIDコントローラーの3つのコンポーネントすべてを見つけます。 比例（P）はエラーそのものです。 Integral（I）-統合するだけです。 微分（D）-理論によれば、誤差を微分する必要があります。 しかし、その中に気圧計と加速度計のひどいノイズがあります。 区別するのは怖いので、トリックを使用します-加速度計によって検出された負の成分をDコンポーネントに使用します。 Dはレギュレーターに減衰を導入するように設計されているため、ここでの速度は非常に適切です-それが多いほど、「消す」必要があります。

各コンポーネントに独自の係数を乗算し、バイアスを追加および取得します。 ただし、ここでは2番目のトリックを適用します。加速に直接バイアスを追加するのではなく、Iパートのみを追加します。 パーマネントエラーの大きさを説明するのは彼女です。これは、バイアスの緩やかな変化という私たちの仮定に対応しています。
PおよびD部分をdTで乗算して速度に追加します（加速度からそれらを借用したため）。
レギュレーターの主なタスクはエラーの定数成分を見つけることであるため、少なくとも短期的な変更に影響を与えるのに十分な「穏やかに」設定することにしました。 しかし、実験のための幅広い分野が残っており、すべてが実際のセンサーの動作によって決定されます。

しかし、ジャイロスコープはどうですか？

上で述べたように、高さを決定するにはジャイロスコープも必要です。 実際、上記のアルゴリズムは、（ローカルシステムの）ベクトルAがZ軸に正確に沿っている場合にのみ機能しますデバイスが傾くと、2つの不快なことが起こります：Z軸上のAの投影が変化し、PIDコントローラーがバイアスを求めてゆっくりと痛みを伴います。 2つ目-水平方向の加速度は、ローカルZ軸にゼロ以外の投影を与え始めます。45°の傾斜角では、加速度がどこにあるのか理解できません。
しかし、グローバルシステムに対するローカルシステムの正確な方向がわかっているため、正義を回復することは難しくありません。ローカルベクトルAをローカルベクトルG（元々は加速度計によって検出され、ジャイロスコープによって慎重に回転された）に投影するだけで、常に地面を覗きます。
この操作は簡単で、ベクトル積の定義に従います。



これは、1Gを差し引く前に行う必要があります。

これで、コードと結果を確認できます。

#define ACC_BARO_CMPF 300.0f #define ACC_BARO_P 30.0f #define ACC_BARO_I (ACC_BARO_P * 0.001f) #define ACC_BARO_D (ACC_BARO_P * 0.001f) #define VEL_SCALE ((1.0f — 1.0f/ACC_BARO_CMPF)/1000000.0f) #define ACC_SCALE 9.80665f / acc_1G / 10000.0f err = (alt - BaroAlt)/ACC_BARO_CMPF; // P term of error errI+= err * ACC_BARO_I; // I term of error accZ = (accADC[0]*EstG.VX + accADC[1]*EstG.VY + accADC[2]*EstG.VZ) * InvSqrt(fsq(EstG.VX) + fsq(EstG.VY) + fsq(EstG.VZ)) - errI - acc_1G; // Integrator - velocity, cm/sec vel+= (accZ - err*ACC_BARO_P - vel*ACC_BARO_D) * cycleTime * ACC_SCALE; // Integrator - altitude, cm alt+= vel * cycleTime * VEL_SCALE; // Apply ACC->BARO complementary filter alt-= err; errPrev = err; 

センサーデータには次の変数があります。
EstG.V-ベクトルG（方向を見つけるときに以前に取得された）
accADC-加速度計からのデータを消去します
BaroAlt-バロメーターからのデータをcmに変換
出力はaccZ、vel、altです。

ご覧のとおり、アルゴリズムの計算の複雑さは非常に単純であり、Arduinoはそれを簡単に「ダイジェスト」します（特に整数算術に変換した場合、コードは読みにくくなります）。



PS：ビデオには、特定の傾斜角でアルゴリズムを無効にすることについてのフレーズがあり、このためエラーが発生します。 実際、この制限は必要ありません-アルゴリズムは0°〜360°のどの角度でも安定して動作します

ソナーをこのアルゴリズムに接続すると（BaroAltの代わりにSonarAltを使用）、高さ曲線はほぼ完璧に見えます。 したがって、低高度ではソナーを使用し、エラーが発生するか、測定限界に近づくと、バロメーターに切り替えます（ソナーからの信頼できるデータの期間中に高度に同意した後）。

残念ながら、天気はまだ新しいアルゴリズムの飛行試験を許可していません。 結果が表示されたらすぐに、ソナーグラフ、プロジェクト全体のデバッグされたソースコード、およびフライトビデオをレイアウトします。