ドローンの自作|姿勢制御プログラム(PID制御編)

Pocket

ブラシレスモーター 3Dモデル

 Arduinoでドローン(クアッドコプター)の姿勢制御プログラムを自作してます。以前にArduinoからモータの回転方法を紹介したのですが、今回は機体姿勢をモーター回転数から制御する部分(PID制御)のアルゴリズムを紹介したいと思います。

これまで製作日記はこちらのページに纏めていってます)

 

▼テスト飛行の様子▼

 ラジコンがほとんど未経験で・・。操縦難しいです。(使い回しの動画ですがすみません)

 

制御プログラム全体の概要

 今回は、「PID制御」の部分。機体の角度(傾き)からモータ回転数を決定する部分です。

 

モーター回転数と姿勢の関係

 まずはモーター回転数と姿勢の概要です。説明の前提として×字型の4個モーターのドローンです。

 

▼上昇下降▼

 4個のモーター全ての回転数を上げれば上昇。下げれば下降です。

 

▼左右移動▼

 左にある2個のモーターの回転数を上げて、右にある2個のモーターの回転数をさげれば、右へ傾き、右へ移動します。左に移動したい場合は逆にします。

 

▼前後移動▼

 後ろのモーターの回転数を上げて。前のモーターの回転数を下げれば前進します。後退はその逆です。

 

▼旋回▼

 ペラ(モータ回転)の反作用で機体が旋回します。上の絵のように右回転のモーター回転を上げると反時計まわりに回転します。

 

 ということで考え方は割と単純です。姿勢を制御する場合は、モーター回転数を機体の傾きに合わせて変えてやればいいわけです。

 

 単純に4個のモーター全てを同回転数で廻してあげれば?とも思いますが、モーター、プロペラの固体差や、外乱(ノイズや風・・など)、機体バランスなどなどいろいろな要因があるため、そんなに簡単ではありません。浮かすことは可能でしょうが、コントロールどころではないと思います。

 

 そこで機体の傾き(姿勢)に合わせて、それぞれのモーター回転数をリアルタイムに制御する必要があります。

6軸センサーから姿勢角の算出方法

<スポンサーリンク>

 

 各軸roll、pitch、yaw角度として表現すると、機体を水平に保つための4個のモーター回転数は、

左前モーター = throttle - roll + pitch + yaw 
右前モーター = throttle + roll + pitch - yaw 
左後モーター = throttle - roll - pitch - yaw 
右後モーター = throttle + roll - pitch + yaw 

(±はセンサー取得値により変わると思います。ここでは一例です)

 イメージですが、基本的にはこんな考え方で行いました。(いくつかオープンソースのフライトコントローラーを参考にしてます。)各軸廻りの角度をミックスしてモーター回転数に反映するイメージです。

 

 ただこのままですとPID制御で言う所のP制御(比例項:目標に対する差分)のみでの制御となります。比例項のみで制御すると機体が暴力的な暴れ方をしてしまい大変危険な目に合うか、どこかにすっ飛んでいってしまうため注意が必要です。

 

 そこで安定した飛行を実現するためにPID制御を行います。(私が参考にしたオープンソースのフライトコントローラーは全てPID制御でやってました。)

 

Arduinoで簡単なPID制御を試してみる

EXCELでPID制御をシミュレーション

 

PID制御

▼PID制御のイメージ▼

 イメージです・・・。roll、pitchの制御です。「角速度のPID制御」の外側に「角度のPI制御」ループを設けてます。

 

 一旦角度制御で「目標となる角速度」を決めて角速度をPID制御してます。このような制御で行えば、送信機で無操作時には水平を保つような制御が行われます。

 

 ラジコン操作の技術がそこそこあれば角速度制御だけでも飛行させることは十分可能かと思いますが・・。私には無理です・・。

 

 微分項は「微分先行型」で行ってます。急な外乱でも大きな操作量が発生しないはず?・・。ただ普通の微分項と比較していないので効果はよくわかりません・・。積分項はたまり過ぎると反応が悪くなりそうなのでソフトで飽和しないように制限かけてます。

 

 roll、pitchは上記アルゴリズムですが、yaw回転の制御に関しては「角速度のPI制御」のみで行ってます。それでも十分安定してました。

 

 こういった感じで算出した「制御量」で各モーター回転数を制御して安定した飛行を行います。記事冒頭の動画はこの考え方でプログラム組んで飛ばしてます。姿勢制御のみなので、風などで当舵あてたり、上昇下降も細かな操作技術が必要となってしまいますが・・。

 

 なるべく自律飛行を目指して、これからセンサー増やすなど、、アルゴリズムについてもまだまだ改良していくつもりです。

 

 ほんとは具体的ソースも例に絡めて紹介したかったのですが・・・。ソースの説明は難しいし、、私の文章表現力が拙くて・・・、簡単に説明できそうにありません。この手の内容はそもそもあまり需要がないかな・・。

 

これまでのドローン製作記録はこちら

Pocket

<スポンサーリンク>


この投稿へのコメント

  1. つむ said on 2018-02-16 at 17:09

    はじめましてimoさん
    僕は高校でドローンを使って研究をやっている者です。充電を無人で行えるようにする研究です。 現在は姿勢制御の段階ですが、Genuinoだとクロックが追いつかずなかなか苦労しています。
    ですのでimoさんの研究はとても参考になり、価値のあるものだとおもいます。
    どのようなプログラムでimoさんが飛ばしているのかかなり気になります。是非、ソース解説をご検討していただけませんか?

    • imo said on 2018-02-16 at 20:22

       つむさん。はじめまして。
      高校生でドローンの姿勢制御ですか・・。素晴らしいですね。Arduino(Genuino)でも十分制御できます。頑張って下さい。
       さてソース解説の件、内容が難しく幅広い上、ソース解説となるとこれまた難しく・・。つむさんが困っていることを具体的に教えていただければ、そこに絞って投稿することもできるかもしれません。

  2. つむ said on 2018-02-19 at 01:58

    ループの中にPWM制御を組み込んでいるので、同じループにPID制御を入れると処理時間分の遅延が出力に影響してしまいます。
    また、ジャイロからの情報処理もイマイチです。空間ベクトルで考えて一気に姿勢制御するのか、3軸それぞれについて一つずつ姿勢制御していくのか……

    • imo said on 2018-02-19 at 19:17

       私もPWM制御はループ内で処理してます。各種演算処理は、よくある高速化の手法を結構使ってます。(平方根、三角関数の近似値、因数分解やなるべくfloat使わないとか・・。)
       ジャイロの情報処理ですが、設計は十人十色かと思います。何が正解ともなく全てが正解とも言えるかと・・。私の場合は演算は空間ベクトル使って、制御は3軸それぞれで処理してます。(「一気に姿勢制御」のニュアンスがいまいちわかりませんが。)ここら辺の内容はそのうち機会があれば解説記事を投稿したいと思います。

コメントを残す

メールアドレスが公開されることはありません。

この投稿へのトラックバック

トラックバックはありません。

トラックバック URL