はてなブックマーク

Arduinoでドローン（クアッドコプター）を製作をしてます。制御基板（回路）、制御プロクラムは自作。フレーム、モーター、バッテリーなどは既製品使って自作してます。そのうちフレームも自作したいです・・。その製作記録（日記）です。

6軸センサー（MPU6050などのIMUセンサー）から、3軸回転を考慮した傾き角度（姿勢角度）の算出方法、補正方法（ジャイロドリフト補正）はいろいろあると思いますが今回は相補フィルターを例に姿勢推定を解説したいと思います。（ここら辺りの内容は散発して記事を投稿していたのですが、一度纏めたいと思います）

概要 制御するものですが、Arduino単体で手軽に試せるように、PWMのDuty比を「操作」して、発生する電圧を「制御」するというものです。Duty比をそのまま制御しても良かったのですが、面白味がなかったのであえて電圧にしてみました。（ほとんど変わりませんが・・・。） PWM出力用の3番pinとアナログ入力の0番pin（AN0）をブレッドボード用のワイヤー等で直差ししておくだけで準備はO.Kです。後はUSBでPCと繋いでおくだけ。 3番ピンから出力されたPWM信号をAN0番ピンで読み取って電圧を測定。PWM信号を操作して目標となる電圧へ制御するものです。内容は、ArduinoのPIDライブラリにあるデモ内容とほぼ同様です。今回ライブラリは使用しません。PIDライブラリは非常に簡単で便利なのですが、痒い所に手が届かないというか・・・。ですので今回は使用しません。 以下のソースや検証結果はU

1軸回転の場合 ▲上図のように、絶対座標系をx-z平面で見て、y軸廻り（pitch角）にθ回転しているセンサーを考えます。検出される加速度をセンサー座標系（加速度センサー）の ax、az としてます。 重力方向をz軸としているのでxz平面で図示してます。回転方向やベクトルの向き等で符号（±）は反転するので使用するセンサーに合わせて適宜調整してください。 算出方法です。1軸回転は単純な三角関数で計算可能。上図からy軸廻り（pitch）の回転角度は三角関数より、 \begin{align} pitch=\cos^{-1}(az)\\ \end{align} もしくは \begin{align} pitch=\sin^{-1}(ax)\\ \end{align} で算出可能です。 ただこれだと検出角度の範囲によって、加速度センサーの分解能の影響が出てしまいます。そこで式を少しいじって、 \beg