加速度センサーで距離を推定する

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

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

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センチのズレが生じ、当然のことながら距離が長くなれば、誤差も大きくなっていきます。

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

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

 

 

関連記事

  • 移動量センサーを実験する移動量センサーを実験する ロボットの移動量測定には、フォトインタラプタなどによる車輪の回転量計測や加速度センサー等によるものなどがありますが、非接触で移動量を検出す […]
  • ジャイロセンサーを使うジャイロセンサーを使う 小型の電動ヘリが出始めの頃、ジャイロセンサーは高級なイメージだったように思います。 ジャイロといえば、地球ゴマが一定の姿勢を保つ動きが連想 […]
  • フォトインタラプタで距離を測定するフォトインタラプタで距離を測定する 加速度センサーによる距離の推定結果が思わしくなかったため、オーソドックスなフォトインタラプタ(フォトリフレクタ)を使用し、距離を測定するこ […]
  • WiringPiでPWM制御するWiringPiでPWM制御する Raspberry […]
  • pigpioでPWM制御するpigpioでPWM制御する PWMによる速度制御は「WiringPiでPWM制御する」で実験しましたが、致命的な問題により実用とはなりませんでした。 他に良い方法はな […]
  • HMC6352 ディジタルコンパスモジュールの実験HMC6352 ディジタルコンパスモジュールの実験 遠隔操作の際、車体の向きを知ることはとても重要です。 ジャイロセンサーは相対的な向きですので、現在の方位を得るためにコンパスモジュールは搭 […]

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です