自作ドローンの制御系設計 vol. 1

Multicopter

今回の記事では、自作ドローンを飛ばすにあたって行った制御系設計に関する試行錯誤の記録をまとめます。

本当はもう少しドローンの制御パフォーマンスを良くしてから記事を書きたかったのですが、来月から別の開発に移りたいと思っているので、一旦この段階で記事をまとめることにしました。将来的にもう少し理論的にもちゃんとした制御系設計をしたいという思いを込めて、今回のタイトルには vol.1 とつけておきました。

ソースコードはコチラ。

概要 & 結果

もともと試していたのは、姿勢角に対する PID 制御を行う手法でした。動画は以下です。

自作ドローン テストフライト 角度制御

なんかちょっとふらついています。

この動画を Twitter で投稿したところ、どろヲタさんと Kouhei Ito さんから角速度フィードバックをインナーループに入れた方がよいのではないかとのコメントをいただきました。

そこで、角速度制御ループを入れてみたり、ローパスフィルタを入れてみたり、トリム調整を行ってみたり、といろいろな実装を加えてみて、最終的に以下のようなパフォーマンスになりました。ふらつき自体はまだちょっと残っていますが、滞空時間が結構長くなっています。

自作ドローン 制御則改善ver

姿勢角に対する PID 制御

ブロック線図

上記のツイートにも載っていますが、改めて清書するとブロック線図は以下のとおりです。

チューニング

PID 制御のチューニングですが、はじめ P 制御のみでやっていたときはこの動画のような感じでまったく安定しませんでした。（角度制御しかしておらず、角速度に対する制御がない状態となっていました。）

D 成分を入れてみて一応形になりました！

ゲインを大きくすると振動的になるが、小さくすると制御が効かず予期しない方向に飛んでいってしまうため、PID パラメータチューニングは結構大変でした。

最終的には、ロール・ピッチ・ヨーの PID ゲインを以下のようなパラメータにしました。

  • $K_p \ = \ 20$
  • $K_i \ = \ 0.2$（ヨー角のみ $0.25$ ）
  • $K_d \ = \ 15$

角度 + 角速度フィードバック制御

ブロック線図

どろヲタさんに紹介してもらった PX4 User Guide を参考にブロック線図を改めて作り直してみました。

チューニング

試行錯誤をたくさんして最適なゲインを見つけていきました。本来、シミュレーション等である程度値の目星をつけてから実験する方が望ましいと思います。

  • 角度制御：$K_P \ = \ (1.5, 1.5, 1.0)$
  • 角速度制御：$K_P \ = \ (0.5, 0.5, 0.5)$

制御に明るい方にアドバイスいただいて、積分項のゲイン $K_I$、微分項のゲイン $K_D$ はゼロにしてみました。

実装上の工夫点

以下の機能を実装しました。順に解説していきます。

  • ローパスフィルタの導入
  • センサのバイアス除去
  • モータの Thrust curve の修正
  • 姿勢角の推定（madgwick filter）に磁気センサを用いる
  • モータ出力のトリム調整を行う
  • 制御周期の見直し

ローパスフィルタの導入

ノイズ除去のため、離散のローパスフィルタを実装しました。実装の方法に関してはこのサイトを参考にしました。

以下のコードで、ロール・ピッチ・ヨー角それぞれに対してローパスフィルタをかけます。簡単に言うと、前回のフィルタリング値にも一定のゲインをかけて現在値とフュージョンする、という処理をしています。

    float kpre = kpre = 0.4;

    for (int i=0; i<3; i++) {
        filtered_data[i] = kpre*pre_filtered_data[i] + (1.0f - kpre)*cur_data[i];
        pre_filtered_data[i] = filtered_data[i];
    }

本来、フィルタリング用の kpre はカットオフ周波数から適切に求める必要があると思いますが、ひとまず暫定で 0.4 に設定しました。

センサのバイアス除去

ジャイロセンサ・加速度センサは基本的にシステムを起動した瞬間には 0 になっているはず（加速度センサ z 軸は 9.8）ですが、すこしバイアスが乗っている場合があります。このバイアスを除去するために、一定期間バイアスを計測して計測後はバイアスを間引く処理を実装しました。

以下のソースコードでは、カウントが 100 ~ 200 までの間はバイアスを計算、200 以降ではバイアスの平均値を減算するという処理をしています。

#define IMU_CNT_START_NUM 100
#define IMU_CNT_TOTAL_NUM 100

    if (cnt > IMU_CNT_START_NUM && cnt <= IMU_CNT_START_NUM + IMU_CNT_TOTAL_NUM) {
        xAcclBiasSum += xAccl;
        yAcclBiasSum += yAccl;
        zAcclBiasSum += zAccl;
    } else if (cnt > IMU_CNT_START_NUM + IMU_CNT_TOTAL_NUM) {
        xAcclBiasAve = xAcclBiasSum / IMU_CNT_TOTAL_NUM;
        yAcclBiasAve = yAcclBiasSum / IMU_CNT_TOTAL_NUM;
        zAcclBiasAve = zAcclBiasSum / IMU_CNT_TOTAL_NUM;

        xAccl -= xAcclBiasAve;  //  Full scale = +/- 125 degree/s
        yAccl -= yAcclBiasAve;  //  Full scale = +/- 125 degree/s
        zAccl -= zAcclBiasAve - 9.8;  //  Full scale = +/- 125 degree/s
    }

    cnt++;

ロール・ピッチ・ヨー角の計算値は、2000 msあたりでジャイロ・加速度センサのバイアス除去が実行されて、0 に近い値になっていることがわかります。（システム起動から計測の間、機体は常に水平を保っています。）

モータの Thrust curve の修正

推力の PWM 調節をジョイスティックからの入力と線形な関係にしてしまうと、高い高度での推力指令値の微調整が難しくなります。そこで、ジョイスティックの入力値が大きい区間では推力指令値の変化をなだらかにし、小さい区間では推力指令値の変化を急峻にするようにしました。

イメージとしては以下のような感じです。

ちなみに、大学時代にドローンの研究で使っていた OpenPilot というソフトウェアに似たような機能がついていました。

姿勢角の推定（madgwick filter）に磁気センサを用いる

ヨー角がドリフトしてしまう現象が発生したので、madgwick filter で姿勢角を推定する際に磁気センサの読み取り値も扱うようにソースコードを改変しました。以下のように、磁気センサを使った場合はドリフトが見事に除去されていることがわかると思います。

磁気センサを使わない場合の姿勢角の推定値（緑：ヨー角）
磁気センサを使った場合の姿勢角の推定値（緑：ヨー角）

改変したと言っても madgwick filter の実装は以前も紹介したこのライブラリを頼っているので、使う API を変えただけです。コメントアウトしている行が磁気センサを未使用時のコードです。

    calculate_mag();
    madgwick.update(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl,xMag,yMag,zMag);
    //madgwick.updateIMU(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl);

ただ、この修正を行った直後、「受信コマンドを適切に処理できない」というバグが発生してしまいました。はじめはハードウェア不良を疑いましたが、実際には磁気センサ値の読み取りを行うことで計算処理に時間がかかってしまっていたことが原因でした。計算処理の途中で次のコマンドを送信機から受信してしまうことで、コマンドの読み取り不良が生じていたようです。送信機からのデータ送信周期を長くすることで、とりあえず解決しました。

ちなみにこうへいさん曰く、地磁気センサを使わずヨー角の傾き計測等でドリフトをキャンセルするのは結構きびしいそうです。温度が変わるとドリフトの傾きも変わるみたいですね。

モータ出力のトリム調整を行う

こうへいさんと洗濯バサミさんからいただいたアドバイスで以下のようなものがありました。一定の方向に偏ってドローンが飛んでいってしまう場合に、制御入力にバイアスをかける（トリム調整する）ことでこの現象を回避できるといったものです。

これを参考に以下のように offset を加えたら、それなりに安定して飛ぶようになりました！

    double offset_motor[4] = {22.0f, 0.0f, 23.0f, 32.0f};
    motor_data[0] = + ctl_data[0] - ctl_data[1] - ctl_data[2] + offset_motor[0];
    motor_data[1] = + ctl_data[0] + ctl_data[1] + ctl_data[2] + offset_motor[1];
    motor_data[2] = - ctl_data[0] + ctl_data[1] - ctl_data[2] + offset_motor[2];
    motor_data[3] = - ctl_data[0] - ctl_data[1] + ctl_data[2] + offset_motor[3]

制御周期の見直し

もともとは、以下の delay() 関数で制御周期を設定しているつもりでした。

#define SAMPLING_TIME_MS 10
delay(SAMPLING_TIME_MS);

しかし、どうやら上記の指定時間に加えて、計算処理で時間がかかっているようです。そこで、上記の delay() 関数を除いて以下の関数を仕込んで、計算にかかっている時間を計測してみたところ、一回の制御ループでおおよそ 14ms の時間がかかっていることがわかりました（参考）。

    unsigned long time = millis();
    Serial.print("time: ");
    Serial.println(time);

なので、上記の delay() 関数は使わずに制御周期を 14ms とすることにしました。madgwick filter のサンプリング周期もこれに合わせて 14ms としています。

今後の改善策

今後の改善策としては、以下が考えられるかなと思っています。

  • フレームをすこし大きくする。（慣性モーメントを大きくしてふらつき軽減。）
  • 姿勢計測周期を短くする + 正確にする。（割り込み機能を使うなど）
  • シミュレーションでゲイン調整の目星をつけられるようにする。
  • クォータニオンベースの制御器やその他制御器を試す。

まとめ & 感想

今回の制御系設計ではいろいろな方にアドバイスをいただきながら、少しずつパフォーマンスを改善していくことができました。みなさまありがとうございました。

途中でドローンが壊れてしまうなどいろいろ大変でしたが、ひとまずある程度安定した飛行ができてよかったです。

コメント