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

昨日、ロボットが「倒れる、倒れない、倒さない問題」と言うのを考えたと、その内容をここに書いた。答えは出ていなかった。
今日、ちょっと昼寝をして起きたら、答えを思いついたので、メモを書いた。間違いを見つけたら、教えていただきたい。
この問題とそのシミュレーション解については、こちらの記事に詳細を示しておいた。論文もダウンロードできる。

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 イニシャライズ
は、マニュアルによると、電源起動時にやっているようなのだが、念のためにやっておいた。