Arduinoでドローン（クアッドコプター）の姿勢制御プログラムを自作してます。以前にArduinoからモータの回転方法を紹介したのですが、今回は機体姿勢をモーター回転数から制御する部分（PID制御）のアルゴリズムを紹介したいと思います。
（これまで製作日記はこちらのページに纏めていってます）
▼テスト飛行の様子▼
ラジコンがほとんど未経験で・・。操縦難しいです。（使い回しの動画ですがすみません）
制御プログラム全体の概要
今回は、「PID制御」の部分。機体の角度（傾き）からモータ回転数を決定する部分です。
モーター回転数と姿勢の関係
まずはモーター回転数と姿勢の概要です。説明の前提として×字型の4個モーターのドローンです。
▼上昇下降▼
4個のモーター全ての回転数を上げれば上昇。下げれば下降です。
▼左右移動▼
左にある2個のモーターの回転数を上げて、右にある2個のモーターの回転数をさげれば、右へ傾き、右へ移動します。左に移動したい場合は逆にします。
▼前後移動▼
後ろのモーターの回転数を上げて。前のモーターの回転数を下げれば前進します。後退はその逆です。
▼旋回▼
ペラ（モータ回転）の反作用で機体が旋回します。上の絵のように右回転のモーター回転を上げると反時計まわりに回転します。
ということで考え方は割と単純です。姿勢を制御する場合は、モーター回転数を機体の傾きに合わせて変えてやればいいわけです。
単純に4個のモーター全てを同回転数で廻してあげれば？とも思いますが、モーター、プロペラの固体差や、外乱（ノイズや風・・など）、機体バランスなどなどいろいろな要因があるため、そんなに簡単ではありません。浮かすことは可能でしょうが、コントロールどころではないと思います。
そこで機体の傾き（姿勢）に合わせて、それぞれのモーター回転数をリアルタイムに制御する必要があります。
各軸roll、pitch、yaw角度として表現すると、機体を水平に保つための4個のモーター回転数は、
左前モーター = throttle - roll + pitch + yaw 右前モーター = throttle + roll + pitch - yaw 左後モーター = throttle - roll - pitch - yaw 右後モーター = throttle + roll - pitch + yaw
（±はセンサー取得値により変わると思います。ここでは一例です）
イメージですが、基本的にはこんな考え方で行いました。（いくつかオープンソースのフライトコントローラーを参考にしてます。）各軸廻りの角度をミックスしてモーター回転数に反映するイメージです。
ただこのままですとPID制御で言う所のP制御（比例項：目標に対する差分）のみでの制御となります。比例項のみで制御すると機体が暴力的な暴れ方をしてしまい大変危険な目に合うか、どこかにすっ飛んでいってしまうため注意が必要です。
そこで安定した飛行を実現するためにPID制御を行います。（私が参考にしたオープンソースのフライトコントローラーは全てPID制御でやってました。）
PID制御
▼PID制御のイメージ▼
イメージです・・・。roll、pitchの制御です。「角速度のPID制御」の外側に「角度のPI制御」ループを設けてます。
一旦角度制御で「目標となる角速度」を決めて角速度をPID制御してます。このような制御で行えば、送信機で無操作時には水平を保つような制御が行われます。
ラジコン操作の技術がそこそこあれば角速度制御だけでも飛行させることは十分可能かと思いますが・・。私には無理です・・。
微分項は「微分先行型」で行ってます。急な外乱でも大きな操作量が発生しないはず？・・。ただ普通の微分項と比較していないので効果はよくわかりません・・。積分項はたまり過ぎると反応が悪くなりそうなのでソフトで飽和しないように制限かけてます。
roll、pitchは上記アルゴリズムですが、yaw回転の制御に関しては「角速度のPI制御」のみで行ってます。それでも十分安定してました。
こういった感じで算出した「制御量」で各モーター回転数を制御して安定した飛行を行います。記事冒頭の動画はこの考え方でプログラム組んで飛ばしてます。姿勢制御のみなので、風などで当舵あてたり、上昇下降も細かな操作技術が必要となってしまいますが・・。
なるべく自律飛行を目指して、これからセンサー増やすなど、、アルゴリズムについてもまだまだ改良していくつもりです。
ほんとは具体的ソースも例に絡めて紹介したかったのですが・・・。ソースの説明は難しいし、、私の文章表現力が拙くて・・・、簡単に説明できそうにありません。この手の内容はそもそもあまり需要がないかな・・。
コメント
はじめましてimoさん
僕は高校でドローンを使って研究をやっている者です。充電を無人で行えるようにする研究です。 現在は姿勢制御の段階ですが、Genuinoだとクロックが追いつかずなかなか苦労しています。
ですのでimoさんの研究はとても参考になり、価値のあるものだとおもいます。
どのようなプログラムでimoさんが飛ばしているのかかなり気になります。是非、ソース解説をご検討していただけませんか？
つむさん。はじめまして。
高校生でドローンの姿勢制御ですか・・。素晴らしいですね。Arduino（Genuino）でも十分制御できます。頑張って下さい。
さてソース解説の件、内容が難しく幅広い上、ソース解説となるとこれまた難しく・・。つむさんが困っていることを具体的に教えていただければ、そこに絞って投稿することもできるかもしれません。
ループの中にPWM制御を組み込んでいるので、同じループにPID制御を入れると処理時間分の遅延が出力に影響してしまいます。
また、ジャイロからの情報処理もイマイチです。空間ベクトルで考えて一気に姿勢制御するのか、3軸それぞれについて一つずつ姿勢制御していくのか……
私もPWM制御はループ内で処理してます。各種演算処理は、よくある高速化の手法を結構使ってます。（平方根、三角関数の近似値、因数分解やなるべくfloat使わないとか・・。）
ジャイロの情報処理ですが、設計は十人十色かと思います。何が正解ともなく全てが正解とも言えるかと・・。私の場合は演算は空間ベクトル使って、制御は3軸それぞれで処理してます。（「一気に姿勢制御」のニュアンスがいまいちわかりませんが。）ここら辺の内容はそのうち機会があれば解説記事を投稿したいと思います。
コメント失礼します。
現在、学生でドローン開発をしています。
imoさんの記事が大変参考になり活用させて頂いております。
一つご相談したいことがありまして、PIDのゲインについてです。
角速度でPID（微分先行型）制御をしており、1ループあたり3.3ms程になっています。
室内でしか実験ができないので、ドローンの底と地面を紐で繋ぎどこかに飛んでいかないような形で実験しています。
P→D→Iの順番でゲインを設定しているのですが、最初の3秒くらいは安定して飛ぶのですが、その後段々と振動が強くなっていってしまい、手で抑えないと墜落してしまいます。
これはD成分が弱すぎると考えて良いのでしょうか。それとも姿勢制御プログラム自体に問題があるのでしょうか。
長文になり申し訳ありません。
ご回答お待ちしております。
Ruinさん、サイト拝見下さり有難うございます。
PIDチューニングですが正直わかりません。私もかなり試行錯誤した記憶があります。既にご覧になったかもしれませんが、以下ページにある動画のように完全に機体を宙づり固定してゲインを現物合わせしてました。動画のような状態で機体が大暴れ（高速回転）して大変危険を感じた覚えがあります。憶測で申し訳無いですが、現象からI成分のゲインが悪さしているような気もします。一度、I成分を非常に小さく設定するか、無くしてしまって試してみてはいかがでしょうか。
https://garchiving.com/pid-tuning-and-checking-algorithms-of-quadcopter/