倒れるときの加速度センサーデータ、対応猶予時間

センサー(SPI通信)とサーボ制御(C2I通信)が別スレッドで、正常にできる様になった。
COSMで、サーボ制御とセンサー制御の両方のコマンドを新たに加えた。このコマンドで、ロボット歩行中のセンサーデータが取れる様になった。
ロボットが倒れる状況のデータが取れた。正確には、倒れるのを手で支える瞬間のデータが含まれているという意味だが。

こちらが、25秒間、10ミリ秒ごとにデータを取った図である。中央に、全ての軸が上下に大きく揺れているのが倒れるのを支えた瞬間のデータである。
その前後(500個分くらい)だけを表示させると以下の様になる。

こう見ると、倒れそうなことを一番特徴的に予見させるのがZ軸である。つまり、縦方向である。
倒れる状況になったのが、1132時点、つまり動き始めから11秒32の時点である。Z軸がおかしくなり始めたのが1107、つまり11秒07である。したがって、ほぼ250ミリ秒前に倒れそうになり始めたことがわかるのだ。1107の時点では、まだ予見できない。「注意」段階。はっきりと沈み込みがわかるのは、おそらく1116あたりだ。つまり、おかしくなり始めて、90ミリ秒で「倒れそうだ!!」という「警告」が出せそうである。倒れるまで、まだ、160ミリ秒もある。
倒れる160ミリ秒前に警告が出たら、なんとかなりそうなきがする。気象庁の地震や津波や台風の警報の様なものである。
ただ、この図では「どちらに倒れようとしているのか」が、わかりにくい。(1)X軸(+が前方)は、プラス側に偏っているので、前方に倒れそうだというのは、わかるかもしれない。(2)Y軸(+が右側)も、徐々にプラス側になっているので、右側に倒れそうだという予測がつくのかもしれない。(実際、その方に倒れたのだが、笑)
では、このとき何ができるのか?
(1)重心のある足を確認する。これは、本来、足の裏にでも圧力センサーをつけて、確認できる様にしたいが、現時点では安易すぎる解決だ。やはり、動かしているサーボモータの状況から理論的に予想できる「重心足」をチェックする。簡単だ。
(2)経験からだが、重心足の縦方向の姿勢を決定的に決めるのは、人間で言えば、足首の角度である。人間の場合は少し違うが、このロボットの場合は、重心足のくるぶし角度は決定的に重要だ。くるぶしの角度を規定しているのは4つのサーボモータである。これらのサーボ角度を微妙に転倒回避の方向に動かす。
(3)残りの全体もまた、影響する。なるべく、倒れる方向とは逆の方向に重心を持っていける様に調整する。どうするかはまだわからないが。
あと、スレッドが問題である。センサーを常時監視しているスレッドと、サーボモーターを制御しているスレッドが別々だ。センサーを監視しているスレッドが、直接サーボをいじると、トラブルになりそうだ。なるだろう。たとえ、mutexなどで、片方をロックしたりしてもダメだ。だから、サーボを制御しているスレッドが、センサーの警告を受け止める適切なメカニズムを作らなければならない。
センサースレッドが警告情報をどこかに書き込み、サーボスレッドがそれを細かく読んでチェックしているという状況だと、書き込みのバッティングも起こらない様な気がする。