RaspberryPi4で遊んでみる(4)-姿勢を制御ってどうすんの？-

2020年8月2日

９軸センサーで遊ぶ

とりあえず、わからぬ言葉を調べつつ、何をすればいいのか考えます。

センサーの数値をそのまま使うってのは無理があるらしい

６軸というと

ジャイロセンサー
加速度センサー

で９軸だとこれに加えて地磁気センサーが加わる。
え、もうこれでOKじゃんと思ったんだが、一筋縄では行かないらしい。ってことでネットをあさりまくった。

センサーの良いとこと悪いところとか

ジャイロセンサ（deg/s）：精度良い。ノイズが原因で発生するドリフト現象によって、時間とともに誤差がじわじわと蓄積してしまう。
加速度センサ（m/s^2）：精度悪い。というか何でも拾ってしまう。運動中でも誤差が蓄積しない。
地磁気センサー(gauss):精度がよいのかどうかはわからない。でも周辺環境に磁力が大きく左右される。calibrationが必要

ざっと、良いとこ悪いところを上げたが一番の問題は、自分の今の状態を直接知ることはできない！ってところ。
すなわち、(1)ジャイロセンサーの値を積分して (2)加速度センサの値と共に補正して(3)ローカルな座標系(roll、pitch、yaw)に変換するってこと。

角度を算出するために補正する方法たち

ジャイロセンサのみ
- 角速度を積分すれば角度になるので、積分すれば良い話だけど、さっきの通りでずれまくる
加速度センサーのみ
- 加速度２回積分すれば距離になるので、かろうじて角度はわかる。でも、重力加速度がデフォルトでついてくる＆３軸になると計算が複雑
相補フィルター
- ジャイロセンサからの値と加速度センサーからの値の線形結合。つまり静的な補正。係数は経験的に決定される。
- 事前にLPF,HPFとおしてノイズはある程度消す必要はあるが、そもそも静止している状態でもノイズ有るのでつらい
カルマンフィルタ
- 統計解析でも用いられる時系列分析のアレ
  状態をサンプリングして状態空間モデルを作成。予測ステップと更新ステップの繰り返す。
  モデルには誤差項が含まれているので、ここでノイズを考慮することと同義。過去値に基づいてモデル化されるので動的に決定されるモデルといえる。
- カルマンフィルター拡張版は中身の式が非線形になっている！（この世界は線形でない！ので現実的と言える）
Madgwick
- 姿勢制御の割と最新の論文？手法？らしい。中身の方は詳しく読んでいないが、四元数という概念が必要らしい。
- 四元数というのも初耳なので、一応しらべた。ベクトルの回転のときに使うんだ…固有値とかのときに出てきてほしかったな
  - https://mathtrain.jp/quaternion
そのた
- 世の中にはいっぱい姿勢角などに関する論文が存在するらしい。GNNとか使っているので、興味があるけどバックグラウンド知識で倒されそうなので省略…
- https://qiita.com/KYoshiyama/items/9f5f5a13f957e138380b

今回はMadgwickを採用してみた

Madgwickってのは、どうにもいい感じの日本語紹介ページがないし、pythonのライブラリも少ない。
あったとしても自分に使えるライブラリが少ない。
ってことで、一生懸命探した結果がこちらのサイト。

https://github.com/morgil/madgwick_py/blob/master/madgwickahrs.py

readmeとかはそんなにしっかりしていないけど、中身が非常に単純明快だったので使ってみた。
（今回自分が使ったセンサーがLSM9DS1で＆安いversionで、これに対応するものは少ない…）
ちなみに、Madgwickでは地磁気センサーの値も考慮する事が可能。

最終的に自分で加工したのは以下のものになりますね。結果を取り忘れたけど、結構いい感じの精度な気がする。

https://github.com/tanico-rikudo/raspi4/blob/master/lsm9ds1/testGetPostureAngleForDrone.ipynb

ところで、オイラー角ってなに？

グローバル座標系とかオイラー角とか出てくるけど、初耳なのでメモ
何度もいうが、自分は機械工学は嫌いだったので基礎教養レベルもしらない身である。

ローカルな座標系(roll、pitch、yaw)ってのは下の通りで、要は進行方向に対する座標系のこと。航空工学の用語らしい。
この座標系に変換しないと行けないってのは、まぁ人間も顔向いている方向ベースで色色考えるのと同じと認識している。

https://qiita.com/KYoshiyama/items/9f5f5a13f957e138380b

さっきのMadgwickで出てくる値はオイラーの座標系だけれども、ここで出てくる値って、実際に運動中の物体にどう生かされるのだろうか？
重力加速度と任意の加速度から許容できる角度（墜落しないで安定を保てる角度）を算出できるようにするのかな？
イメージ的にはあっているとおもうけど、こんな単純な話ではないと思うんだよなぁ…
物体の重量偏りとか周りの環境とかでcalibrationすごくやらないとまずそう…