加速度センサーの精度

ロボットの転倒に関する議論をしてきたが、逆振り子で転倒を回避する場合でも、自らの傾き、ないしは揺らぎを捉えることが不可欠である。ここでは、KXSD9-2050という750円のセンサーを使っているので、どこまで精度が出るのかがいたって不安である。

そこで、より詳細に調べてみた。

KXSD9-2050については、sensitivityを819counts/gに変更したことを除いて、初期設定を採用している。

10ミリ秒に一つのデータを取る計算で、500個のデータを取った。その間に、センサー(RaspberryPiに固定している)を6回だけ、数ミリずらせる速度をY軸方向に与えた。そのうちの50個目から200個目までのデータを図で表したものが以下の画像である。
Y軸だけをみている。gは重力加速度(9.80665 m / s2)である。0.05のところを中心に動いているのは、多少Y軸が傾いていることからくるのか、私の家が傾いているのか、机が傾いているのか、そんなところだろう。ホワイトノイズとして処理する。

数ミリ動かしたことによる影響は、赤と青で色ぬりされた部分に現れている。赤から始まっているのは、Y軸のプラスの方向に加速度が与えられ、マイナスになっているのは、速度が低下して、ほぼ再び止まったことを表している。止まったということは、赤の面積と青の面積がほぼ等しいことを表す。

実際移動した距離は加速度がプラスから始まっているので、Y軸方向に移動してどこかで止まったということで、それまでのプラスの距離になる。

指で数ミリ動かしただけで、これだけのものがとらえられることは、この安物のセンサーも結構使える可能性があることを示している。

今、赤と青の部分が単純な三角形だったとしよう。横幅が8目盛、高さが0.1gくらいになる。この時、最高速度は、

80(ms)X0.1(g)X(1/2)=0.08X0.1X0.5X9.80665=0.039226(m/s)

およそ1秒間に4センチ動くくらいの速度が最高速度だったということになる。

それから速度が0になるまで同じくらいの時間がかかったとしよう。簡単化のために、全て、三角形で捉えよう。そうすると、移動した距離は、

0.08X2X0.039226/2=0.00313808(m)

すなわち、3.2ミリ移動したことになる。私が先に数ミリ動かしたと言ったが、ほぼその値に一致する。

ということは、この加速度センサーが捉えている加速度は、ノイズできなものはありながらも、なかなか、実態を反映しているということなのである。

逆さ振り子としてのロボット

以下の記事についての詳細は論文参照
NAOは、ただ立っているとき、小さく体を揺らしている。これはALDEBALANの技術者が、より生きているように見せようとした工夫だろう。実際そう見える。ただこれが、NAO自身にとって必要なこととは思えない。問題は、なぜ体を揺らすことが生きているように見えるために必要なのかということだ。結局それは、人もまた、立っている時でも体を微妙に揺らせているからだ。人の場合は、体を揺らせることによって直立状態を維持できる、という必要においてそうなのだ。無意識に体の姿勢を制御している。

この揺らせることによって直立を維持するメカニズムの基本的なところは理論モデルとシミュレーションで示してきた。

理論モデルで示したように、直立構造の運動方程式は、単振り子の運動方程式とよく似ている。同じように見える。ただ、加速度の方向が逆向きになっていて、三角関数ではなく指数関数でそれが解けるようになっている。

下向きのたん振り子は、それ自体に、あるいは内生的に、復元力が働くのだが、逆さまの振り子であるロボットは(現在の多くのロボットは、NAOも含めて逆さ振り子とは言えない。新しい設計思想で作られるロボットのことを意味する)、それ自体は、転倒が基本なのだ。それに対する外生的な揺り戻しが生じることによって転倒を微妙に避けている。このような姿勢制御によって、直立が維持される。そのプロセスは、結局、揺れている状態と同じになるのだ。

この揺れは、この立っている状態に冗長性を生むというもう一つの、とても大事な側面を持っている。単なる剛体であれば、ちょっとした撹乱としての傾きで倒れてしまう。しかし、常に揺れながら状態を直して入れば、撹乱を吸収できるゆとりが出てくる。

スポーツでは、膝や腰や腕を柔軟に使うことが大切なこととして求められる。人間のさまざまなしなやかさは、関節の自由度の大きさによって確保されている。

そのようなロボットをイメージすることがとても大切だ。

ロボット転倒問題のシミュレーション解

前の記事で書いたロボットが倒れる、倒れない問題について、シミュレーション解を出すことができた。なお、計算に使ったJAVAの関数は、以下の論文の中に示しておいた。
詳細は論文参照
この図で、横軸は、横の棒の長さを1としたときの高さである。縦軸が、転倒を回避するために、臨界点で止まる(再び位置エネルギーが元の値に戻る)ために必要な関節を固定化する角度である。ラディアンから度に変換している。

計算結果の誤差は、ここで解とみなした値のときで、高さの比が1のときが最も小さく0.0000000082である。高さの比がここで調べた中で最も大きい20.0のときに0.0000011608である。いずれもゼロに極めて近く、ほぼ解であると考えて間違いないだろう。刻み方をもっと細かくすれば、さらに誤差を小さくできることが予想される。ただ、高さが高くなるほど誤差は僅かながら大きくなっている。その理由は、高さが高いと、微妙なバランスが求められると考えても良いだろう。

その結果の特徴は、まず、理由はまだ正直わからないのだが、常にαとβは等しくなる。

高さが1のとき、すなわち、横棒と縦棒が同じとき、角度は29.99697622度、ほぼ30度だ。何か理由がありそうな角度である。αも同じになることは先に述べておいた。綺麗に90度を三等分する角度になっている。ただ、この角度は大きい。つまり、頂点で手を離して、30度傾くまで、自由な関節の回転を放置しておいて良いのだ。その後固定化すると、ギリギリ転倒を回避できる。高さが低いからである。

小さなロボットが転倒しにくい理由はここにある。

また、比率で見た高さが高くなると、最初は急速に関節を固定化するまでの角度が小さくなっている。その後は、少しずつ低下する。すなわち、高さが高くなればなるほど、転倒しないために関節を固定化しなければならないタイミングは早くなるのである。僅かに傾いたら、即固定化ということになっていく。

それでも、高さが10のときに、β=2.870976923度、ほぼ3度の余裕がある。3度傾いたら、関節を固定化しなければならないのである。高いと、微妙な調整が必要になる。

これだけでも面白い結果だが、人に応用するともっと興味深い。例えば、私の身長は182cmである。一方、足のくるぶしの関節から足のつま先までの長さは18cmくらいである。高さの比は、10.1である。

上の計算で、10.1の限界度、βは、2.843990611度である。つまり、足前に出したりせずに、動かさずに立った状態を維持するためには、2.8度、前に傾いたら筋肉を緊張させなければならないのである。

確かに、体を前にわずかに倒そうとすると筋肉を硬直させるが、この角度が、2.8度だよとこのシミュレーション結果は示している。もちろん、人間の体は質量が分布しているので、理論モデルで示したような質量の一点集中とは違っている。

一つ首をかしげることは、常にαとβが、ぴったり等しくなることである。数学モデルから、解析的に言えそうなのだが今の所わからない。

残された課題は、(1)頂点から倒れ始めて、関節が固定され、最終的に転倒の臨界角度に至るまでの時間を示すことである。確かに、高さが高いほど、関節を固定しなければならなくなるまでの角度は急速に小さくなっていく。しかし、そこに至る時間はどうなるかわからない。それを解析に出せないかを考えることである。また、その時間には、B点の質量が影響を与える可能性が高い。メトロノームが重りを上に持っていけばいくほど、スピードが遅くなることも考慮される。(2)実際にシステムを組んでシミュレーションして確かめることである。シミュレーションするための材料はほぼ整っている。加速度センサーも、サーボも、RaspberryPiから制御できるようになっているからである。

ロボットの転倒及び転倒回避に関する基本問題

昨日、ロボットが「倒れる、倒れない、倒さない問題」と言うのを考えたと、その内容をここに書いた。答えは出ていなかった。

今日、ちょっと昼寝をして起きたら、答えを思いついたので、メモを書いた。間違いを見つけたら、教えていただきたい。

この問題とそのシミュレーション会については、こちらの記事に詳細を示しておいた。論文もダウンロードできる。

3D加速度センサーのKXSD9-2050をRaspberryPi 3 から使う

3D加速度センサーのデータの拾い方に苦労したが、これでいいのかもしれないというところまで来たので、記録しておこう。
加速度センサーを起動して、500回分ループ動かした(データを取得した)。1データ100m秒間隔でとっている。

まず、水平においている。縦が、Z軸だから、重力がかかっているので、1gのはずだが、3gになっているのは、測定レンジを6gにしてあるからなのかもしれないがよくわかっていない。確かに、裏返すと-1gで、3gだと-3gで結局幅が6gになるので、それでいいのかもしれない。

まず、X軸に向かって回転しながら、裏返すところまで持って行った。途中ちょっとフラフラした分も描かれているが、Z軸がマイナスに触れた分、X軸に重力がかかっている。次に、Y軸に沿ってマイナス側に回転させたら、確かにY軸はマイナスに触れている。その後、X軸とY軸をそれまでの操作と逆方向に回転させた。図はほぼそれを忠実に再現している。

モジュールのつなぎ方を記録しておく。

モジュールは、KXSD9-2050(画像の右下)だ。RaspberryPi(画像では、下にあるものだが、ボードを1枚被せて、電源5V、電源3.3V、I2Cの拡張コネクタを乗せている)とI2C(アイスクエアシーと発音する)で接続する。

ピン接続は以下の通りだ。
モジュール1→電源3.3V+(新しく5Vから降圧したピンに、Raspberryからとっても良い)
モジュール2→電源のGNDー
モジュール5→RaspberryPiのSCL、GPIO3
モジュール6→電源3.3V+(これをつながないと、I2Cがうまく機能しない)
モジュール7→RaspberryPiのSDA、GPIO2
以上だ。3.3Vを二つ繋がなければならないのが気をつける点だ。

繋いだら、RaspberryPiのターミナルで
i2cdetect -y 1
と打って、モジュールのアドレスを確認しておく。
$ i2cdetect -y 1
0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          — — — — — — — — — — — — —
10: — — — — — — — — 18 — — — — — — —
20: — — — — — — — — — — — — — — — —
30: — — — — — — — — — — — — — — — —
40: 40 — — — — — — — — — — — — — — —
50: — — — — — — — — — — — — — — — —
60: — — — — — — — — — — — — — — — —
70: 70 — — — — — — —
サーボモータ関係のものが二つ繋がっているが、モジュールのアドレスは、0x18となる。

プログラムは、

#include <stdio.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

// コンパイル
// gcc 3dsensor.c -o 3dsensor -lwiringPi

int main(int argc, char **argv)
{
int i2c_fd;             // I2Cデバイスファイル
// sudo i2cdetect -y 1 を実行すればKXSD9-2050のI2Cアドレスを知ることができる
int i2cAddress = 0x18;  // KXSD9-2050のI2Cアドレス
int i;
int xregH,xregL,xout;
int yregH,yregL,yout;
int zregH,zregL,zout;
double xac,yac,zac;

// I2Cデバイスファイルをオープン
i2c_fd = wiringPiI2CSetup(i2cAddress);

// KXSD9-2050 イニシャライズ
if((wiringPiI2CWriteReg8(i2c_fd,0x0a,0xca)) < 0){
printf(“write error register 0x0a: KXSD9-2050 イニシャライズに失敗\n”);
return 1;
}
printf(“write register:0x0a = 0xca\n”);

// WiringPi イニシャライズ
if(wiringPiSetupGpio() == -1){
printf(“WiringPi イニシャライズに失敗\n”);
return 1;
}

// 加速度データを取得
printf(“データを500個取得します\n”);
for(i=0; i<500; i++){
// デバイスからデータ取得
xregH = wiringPiI2CReadReg8(i2c_fd,0x00);
xregL = wiringPiI2CReadReg8(i2c_fd,0x01);
xout = xregH<<4|xregL>>4;
//printf(“xH: %6d  xL: %6d\n”,xregH,xregL);

yregH = wiringPiI2CReadReg8(i2c_fd,0x02);
yregL = wiringPiI2CReadReg8(i2c_fd,0x03);
yout = yregH<<4|yregL>>4;
//printf(“yH: %6d  yL: %6d\n”,yregH,yregL);

zregH = wiringPiI2CReadReg8(i2c_fd,0x04);
zregL = wiringPiI2CReadReg8(i2c_fd,0x05);
zout = zregH<<4|zregL>>4;
//printf(“xH: %6d  xL: %6d\n”,xregH,xregL);

xac = (double)(xout-2048)/(double)273;
yac = (double)(yout-2048)/(double)273;
zac = (double)(zout-2048)/(double)273;
//printf(“X軸 : %6d   Y軸 : %6d   Z軸 : %6d\n”,xout,yout,zout);
printf(“No. %4d : X軸:%6d Y軸:%6d Z軸:%6d ==>> X軸:%10.6f Y軸:%10.6f Z軸:%10.6f\n”,i,xout,yout,zout,xac,yac,zac);
delay(100);
}
return 0;
}

指定したアドレス(仕様書に書いてある)から、データを取得できる。ただ、2バイトで送られてくる。そのうちのHighとLowから、12ビットが使われるのがちょっと面倒。そこで、取得したデータの、Highを4ビットだけ左にシフトさせ、Lowを逆に4ビット右に論理和をとったものをデータとしている。

操作のイメージは次のようになる。有効なビットをA、無効なビットをX、ゼロは0で表そう。
まず、Highは AAAAAAAAを左に4ビットシフトさせてAAAAAAAA0000とする。
次にLowはもともとAAAAXXXXとなっているので、右に4ビットシフトさせて0000AAAAとするわけである。
上記二つの値の論理和をとると、
AAAAAAAAAAAA
と見事に12ビットになるという手筈なのだが、これでいいかどうか確証はない。

このデータから元の加速度を出す方法も大事だ。プログラムの中に書かれているようにやればいいはずだ。まず、オフセット値が与えられている。ここの場合、2048カウントである。得られたデータとこのオフセット値の差をとって、その差を1gあたりのカウント値 (6g幅を使用している場合(デフォルト)は273)で割ることによってgの値が取れるはずなのだ。これも、誰かに確かめているわけではないが、論理的にはそうなるということだ。

これで計算したものが、冒頭の図である。図がほぼそんな感じなので、間違っていないと思う。

なお、プログラム始めの
KXSD9-2050 イニシャライズ
は、マニュアルによると、電源起動時にやっているようなのだが、念のためにやっておいた。

ヒューマノイドロボットとコミュニケーション