ドローン製作記のメインページ 【完全自作】Arduinoを使用した自作ドローンの製作記（完成）
ドローンが飛ぶためには姿勢角を求めるセンサーが必須です．これは，ドローンの中でも非常に重要なパーツです．
プロペラを均等に回せば飛ぶんじゃないかと思っている人もいますが，そんなことしたら1秒で墜落します．というより，離陸すらできないかも？
そこで，ドローンの姿勢を正確に推定していく必要があります．
ちなみに，今回はMPU9250という9軸センサを使用しました．使い方はこちらにまとめてあります．
【9軸センサ】MPU9250の使い方とArduinoプログラム
-
ドローンの状態推定
まず，ドローンの状態推定をして，機体姿勢を推定するには，角速度センサ（ジャイロセンサ）と加速度センサの両方が必要になります．
理論上はジャイロセンサの値を積分することで，機体の姿勢は求まります．しかし，ジャイロセンサにはドリフトやノイズなどの影響を受けるので，時間が経てば経つほど出力値が外れていきます．仮にセンサーをずっと静止させていても，推定値が0度からどんどん外れて行きます．（ドリフトとは：ジャイロが静止している状態での出力のズレのこと）
そこで，このドリフトを補正するために，加速度センサを使用します．加速度センサという名前の通り，重力加速度すなわち地球の重力を測定することができます．地球の重力は常に下向きなので，ジャイロセンサのドリフトを打ち消すことができます．
ならば，加速度センサだけを使えば良いのでは？と思うかもしれません．
加速度センサのデメリットとして，動いているときは正確に重力の方向が測定できません．ドローンは動くものなので，横方向や縦方向，斜め方向に加速度が働きます．そうすると，ドローンの加速度と重力加速度の合成加速度が測定されてしまうのです．
すなわち，
- ジャイロセンサ：短期間での動きは正確であるが，長期間での値は不正確
- 加速度センサ： 動いていると正確に姿勢が求められないが，時間が経っても値は必ず正確
詳しく説明すると，ジャイロセンサの値をハイパスフィルタに通し，加速度センサの値をローパスフィルタに通すことで，信頼性の高いデータを得ることができます．（ジャイロセンサの短期的な値だけを抜き取り，加速度センサの長期的な値だけを抜き取るということ）
このように，違うセンサを組み合わせて使うことをセンサフュージョンと言います．
-
センサフュージョンの種類
センサフュージョンにはいくつか種類があります．ここでは，有名なものだけを説明します．
-
カルマンフィルタ
アポロ計画でも使用されたフィルタなので，知っている人も多いフィルタ．複数の不正確な情報から正確な値を推定する方法．世の中の多くの製品でも使われています．しかし，理解するのが難しく，式が複雑（物体がモデル化できていないと正しい値が得られにくい）．
-
相補フィルタ
先ほど述べた，加速度センサにローパス，ジャイロにハイパスをかけることで信頼できる値を得ることのできるアルゴリズム．式がとても簡単
-
Madgwickフィルタ
比較的最近できたフィルタで，クォータニオンをベースにして作られています．カルマンフィルタと似たようなアルゴリズムですが，計算処理が軽くて，物体がモデル化できていない場合でもカルマンフィルタと同等以上の精度を出すことができます．
結果的に3種類のフィルタを使用しました．次に，実際に試した3種類のフィルタの概要を書いていきたいと思います．
-
プログラム
-
最初に使ったフィルタ：RCフィルタ
最初は，RCフィルタを使用してみましたが，ドローンにはあまり向いてなさそうだったので（直感），やめました．詳しくは，こちらに書いています．
【Arduino】RCフィルタをプログラムで作ってみた
-
2番目に使ったフィルタ：相補フィルタ
最初に使ったプログラムの例を置いておきます．多分，相補フィルタだと思います（相補と呼んでいいのかわからない）．
この例だと，90度以上傾けたり，みそすり運動をするとYaw軸が勝手に回転しちゃいます→正確な値が得られていない．
degX： 推定した姿勢角度
preDegX： 前回算出した角度
gyroX： ジャイロセンサから推定した角度
accDegX； 加速度センサから推定した角度
cutOffFreq：ローパスフィルタのカットオフ周波数ω（ω[rad/s]=2π*f, f[Hz]）
これを使用したところ，まあまあな値がでるようになりました．
ちなみに，ωは自分の場合0.628程度が最適でした．
-
3番目に使ったフィルタ：Madgwickフィルタ
クォータニオンがベースのフィルタです．計算処理がカルマンフィルタより軽いので，マイコンにも優しいです．
このフィルタがとても優秀で，感動するぐらい正確な値を出してくれます．
他のフィルタだと，プロペラの振動で推定角度がおかしくなったりしましたが，これは全く振動の影響を受けませんでした．
最終的には，このMadgwickフィルタを使用しました．
時間がある時に別記事で紹介したいと思います．
【Arduinoドローン】No.5 PID制御プログラムの作成
Madgwickフィルタを用いて姿勢角を推定するにあたり，クォータニオンのドリフトに苦しんでいます．角速度のバイアスなどに対策をされていましたらご教授お願いします．
マイコンの起動時に、MPU9250のジャイロからのデータを数秒間積算し、それを元にドリフトが何deg/minあるのか計算しています。
これで得られたドリフト量を、ジャイロからの生データに付け加えて補正してから、Madgwickフィルターに入れています。
地磁気センサを使用していないで、ジャイロのみで角度を推定していますが、1、2分の飛行なら全然ドリフトしないで飛んでくれます。ドローンの性質上少しぐらいはドリフトしても問題ないのでこの状態で使っています。
ご丁寧にありがとうございます。