Arduinoで倒立振子｜6軸センサーで姿勢角度の算出

　Arduinoで倒立振子です。

　前回、回路基板含むハード部分が完成したので今回からソフトウェア（Arduinoスケッチ）の作成に取り掛かっていきます。（これまでの製作記録はこちら）

概要

　Arduinoでいろいろと多くの処理を行うのですが、今回から少しずつ切り分けて順に処理内容を見ていきたいと思います。

　倒立振子の目的は倒れないようにすること。簡単にいうと倒れそうな方向にタイヤ（モーター）を廻して倒立状態を維持する感じです。

　その「倒れそうな方向」を推定する一つに6軸センサーから取得する情報（自分の姿勢）があります。で今回は6軸センサーから機体の情報を取得して姿勢角度を算出する部分を行ってみました。

▼確認動画▼

　本体の姿勢角度をビジュアル化してます。倒立振子では1軸の回転角度を把握すればいいのですが、せっかく6軸センサーを使用しているので3軸回転の姿勢角度算出をしてます。

　概要ですが今回行ったことは以下です。

　まずはArduinoから6軸センサーの値を取得します。使用するセンサーは「MPU6050」のモジュール品で3軸加速度と3軸角速度が取得できるものです。（ArduinoとMPU6050の使い方（通信方法）はこちら）

　で取得した加速度と角速度から姿勢を推定（計算）します。

　姿勢角度の算出方法はこちらで詳しく説明してます。がリンク先のやり方が少しいいかげんというか、ベクトル合わせが無理やりなので、今回はきちんと？姿勢角度を算出します。

　手順としては、

　を繰り返し更新して姿勢を計算してます。

　「補正」の方法はリンク先の方法と同様に相補フィルターを使ってます。がリンク先とは違って、加速度ベクトルはきちんと回転行列使っ計算してます。四元数も理解したかったのですが、少しとっつきにくかったのでまたの機会にチャレンジしてみたいと思います。

　また角度換算をいちいち行っていると時間がかかってしまうため、更新（補正）は加速度ベクトルで行って、欲しいときに角度換算するようにしてます。

　おおざっぱですがこんな感じ。割とマニアック内容なのとややこしいので細かい説明は割愛です・・・・。要望があれば解説したいと思います。

　今回は、姿勢角度の算出（Arduinoで行う処理）よりも、「無線での情報のやり取り」、「リアルタイムでリアルな3D描写」を実現する方が結構大変でした・・・・・・。