移動量測定 | SPACE LABORATORY西東京　ロボット研究所

ハウスローバーにとって、走行距離の計測は重要な項目です。　最も基本的な方法は、車輪の回転を検知することですが、車輪のスリップや障害物の影響などを受ける方法でもあります。　今回は、加速度センサーを使用し、その測定値の信頼性を検証します。

MPU-9150 9軸センサーモジュール

MPU-9150は、加速度センサー、ジャイロセンサーと電子コンパスまで付いた、お得感たっぷりのモジュールです。　加速度センサーによる距離測定がものになれば、ジャイロセンサーも統合できますので、何かと便利です。

実験用のプログラムは、ジャイロセンサーの角速度から角度を算出したものを応用し下記の通りとしました。

事前にセンサー値を複数回読み取り補正値を算出　→　計測開始　→　加速度を算出　→　加速度を積算し速度を算出　→　速度を積算し距離を算出　　という流れです。

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

#define MOTOR1 22           // GPIO22
#define MOTOR2 23           // GPIO23
#define MOTOR3 24           // GPIO24
#define MOTOR4 25           // GPIO25

int main(int argc, char **argv)
{
	int i2c_fd;       		// デバイスファイル用ファイルディスクリプタ
	int i2cAddress = 0x68;	// MPU9150のI2Cアドレス
	int i,regH,regL,out_x,adj,backup = 0,inp,dis = 0;
	float data,acc,spd;

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

	// MPU9150 イニシャライズ
	if((wiringPiI2CWriteReg8(i2c_fd,0x6b,0x00))<0){
		printf("write error register 0x6b");
	}
	printf("write register:0x6b0 = 0x00\n");

	//引数の取得
	inp = atoi(argv[1]);

	// WiringPi イニシャライズ
	if(wiringPiSetupGpio() == -1) return;

	// モーター停止
	digitalWrite(MOTOR1, 0);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 0);

	// 補正値を計測
	for(i=0; i<20; i++){
		// デバイスからデータ取得
		// read L reg
		regL = wiringPiI2CReadReg8(i2c_fd,0x3c);
		// read H reg
		regH = wiringPiI2CReadReg8(i2c_fd,0x3b);
		out_x = (regH<<24|regL<<16)>>16;

		// 取得したデータを表示
		//printf("i : %d  out_x : %d\n",i,out_x);
		adj = adj + out_x;
		usleep(10000);		//10ms
	}
	adj = adj / 20;			// 平均化し補正値とする
	printf("補正値 : %d\n",adj);
	sleep(1);

	// 前進
	digitalWrite(MOTOR2, 1);
	digitalWrite(MOTOR4, 1);

	// 加速度データを取得、表示
	for(i=0; i<10000; i++){
		// デバイスからデータ取得
		// read L reg
		regL = wiringPiI2CReadReg8(i2c_fd,0x3c);
		// read H reg
		regH = wiringPiI2CReadReg8(i2c_fd,0x3b);
		out_x = (regH<<24|regL<<16)>>16;

		// 取得したデータを表示
		//printf("i : %d  out_x : %d",i,out_x);

		// データを補正し加速度算出
		acc = out_x - adj;
		data = (acc / 16384) * 9.80665;	//加速度を算出
		spd += data * 0.5;				//速度を算出
		if(spd < 1){spd = 0;}
		dis += spd;						//距離を算出

		//printf("  加速度 : %.5f  速度 : %.3f  距離 : %d\n",data,spd,dis);

		//距離をチェックし、指定値に達していたら停止
		if (dis >= inp ){
			//ブレーキ
			digitalWrite(MOTOR1, 1);
			digitalWrite(MOTOR4, 1);
			digitalWrite(MOTOR2, 1);
			digitalWrite(MOTOR3, 1);
			usleep(100000);
			//停止
			digitalWrite(MOTOR1, 0);
			digitalWrite(MOTOR4, 0);
			digitalWrite(MOTOR2, 0);
			digitalWrite(MOTOR3, 0);
			return;
		}
		usleep(10000);		//10ms
	}

	return;
}

30センチの距離を数十回テストしましたが、全く安定しません。　毎回、2～3センチのズレが生じ、当然のことながら距離が長くなれば、誤差も大きくなっていきます。

パラメータや算出方法を変えて行ってみましたが、信頼できる結果は得られませんでした。　残念です。

フォトリフレクタを使用した、車輪の回転数による距離計測を使用すべきだと実感した実験結果となりました。