６軸センサで3軸姿勢制御モジュールの傾き角度を計算

　リアクションホイールで3軸姿勢制御モジュールの自作です。前回、ハードが完成したので今回から本格的に倒立制御の検討に入っていきます。

　倒立制御のおおきなポイントは2つ。CUBEの姿勢（角度）を常に把握することと、その姿勢を水平に維持するために3つのモータの回転制御をすること。今回は角度検出についての内容です。

6軸センサの取付け位置

　ちょっとわかり難いですが、箱の角にセンサ取付けてます。ここに配置したため、センサの校正に少し混乱しました。

CUBEのバランス状態

　最終的には状態でCUBEをバランス倒立させるのが目標。

　点倒立した状態でセンサが水平になるように赤い面のところにセンサを取付けてます。

初期状態

　そのため、初期状態ではセンサが傾いた状態で制御をスタートすることになるため、電源投入時の校正がややこしい。しかも現物のセンサxy軸をCAD座標系とテレコ位置で設計・取付けしてしまったので余計ややこしくなってます。
　CUBE水平時のセンサ座標系（加速度センサ）での重力ベクトルはCADから測定して（-0.4082、-0.7071、0.5774）。その値からオイラー角計算すると（-50.7°、24.1°、-26,6°）になるはずです。

角度を実測

　実際にプログラム書いて試してみました。

　概ね計算値通りです。YAW角（z軸）は計算してません。センサ校正しなくても割と良い精度でした。ただどこで間違えたのかROLL角が正負逆、、。

　素直にCUBEの稜線に沿ってセンサ配置すればよかったのですが、できれば倒立中の姿勢角度を0°としたかったものですから、、。少し寄り道しましたが次回は1軸（１辺）での倒立振子制御を行ってみます。