2017年度 情報科学類 ソフトウェアサイエンス実験B

移動ロボットの行動プログラミング
第3回 測域センサの使い方

更新履歴

講義資料

レーザ測域センサとは

レーザ測域センサは、移動ロボットなどの環境認識に用いる、走査式のレーザ距離センサの総称です。 代表的なレーザ測域センサとしては、筑波大学知能ロボット研究室と北陽電機株式会社の共同研究で生まれたURG(アージ)シリーズ(Fig. 1)、SICK社[独]のLMSシリーズや、Leuze社[独]のRSシリーズ、Velodyne社[米]のHDLシリーズがあります。

Fig. 1: 北陽電機株式会社 URG-04LX

例えば、北陽電機株式会社のURG-04LXでは、正弦波で変調した赤外線レーザ光を発し、測定対象物からの反射光を受信し、その受信した正弦波の位相遅れを測定することで距離を測定します。そして、このレーザ光を、モータで回転させたミラーで走査することで、周囲240度の20mmから5600mmまでの距離データを取得できます。もう少し詳しい測距方法を知りたい場合は、追加資料を参照して下さい。

Fig. 2: URG-04LXの測距原理(位相差方式)

測域センサ用のコマンドインタフェースプロトコル SCIP2.0

北陽電機株式会社のURG-04LXでは、センサの通信プロトコルとしてSCIP2.0(リンク中の「URG-04LX 通信仕様書 (SCIP 2.0)」を参照)を採用しています。 SCIP2.0は、シリアル通信(RS-232C)や、USB CDC-ACMなどのインタフェースを通して、コンピュータとセンサの間でデータのやりとりを行うためのテキストベースのコマンドで構成されています。

まずは試しにSCIPコマンドを使ってみましょう。ターミナルを2つ開き、並べて下さい。それぞれのターミナルで、以下のコマンドを実行して下さい。(MacOSでは/dev/ttyACM0の部分の名前が異なります。)

$ sudo chmod 777 /dev/ttyACM0
$ cat /dev/ttyACM0
	
$ cat - > /dev/ttyACM0

後者のターミナルでSCIPコマンドを入力すると、前者のターミナルにセンサからの応答が返ってきます。以下のコマンドを入力してみて下さい。

SCIP2.0
VV
PP
II
		

それぞれのコマンドで、センサのバージョン情報、測距・走査の情報などが取得できます。続いて、以下のコマンドを入力して下さい。

MD0384038400001

このコマンドは、MD[取得開始角度ステップ(4桁)][取得終了角度ステップ(4桁)][まとめるステップ数(2桁)][間引くスキャン数(1桁)][スキャン回数(2桁)]を意味します。 つまり、384ステップの方向(URG-04LXの正面方向)の距離を、1スキャン分取得する、という意味です。これにより得られた応答は次のような意味を持っています。

MD0384038400001 コマンドエコーバック
00P ステータスコード(00:OK)とチェックサム
MD0384038400000 データ送信開始行(後ろ2桁は残りスキャン回数)
99b ステータスコード(99:OK)とチェックサム
3=CCf タイムスタンプ(4桁)とチェックサム
1DhM 距離データ(3桁)とチェックサム
		

タイムスタンプや距離データは、バイナリの距離データをテキストにエンコードした文字列として取得されます。このテキストのデコード方法について詳しく知りたい場合は、前述のSCIP2.0の資料を参照して下さい。

ライブラリを使った距離データの取得

センサとの通信を確立したり、SCIPコマンドの応答をデコードして数値に直す処理をいちいち実装するのは面倒ですので、ライブラリを使って簡単に距離データを取得しましょう。 libscip2awdを使う場合は、以下のようなソースコードで距離データが取得できます。

#include <stdio.h>
#include <unistd.h>
#include <sys/time.h>
#include <signal.h>
#include <math.h>
#include <scip2awd.h>

int escape;

void ctrlc( int notused )
{
	// MDコマンドを発行したままプログラムを終了すると、
	// 次回起動時に少し余分に時間がかかる
  	escape = 1;
  	signal( SIGINT, NULL );
}

int main( int argc, char *argv[] )
{
	S2Port *port;    // ポート
	S2Sdd_t buf;     // データ取得用ダブルバッファ
	S2Scan_t *scan;  // データ読み出し用構造体
	S2Param_t param; // センサのパラメータ構造体
	int ret;

	if( argc != 2 ){
    	fprintf( stderr, "USAGE: %s device\n", argv[0] );
    	return 0;
  	}

	// ポートを開く
  	port = Scip2_Open( argv[1], B0 );
  	if( port == 0 ){
    	fprintf( stderr, "ERROR: Failed to open device.\n" );
    	return 0;
  	}
	printf( "Port opened\n" );

  	// 初期化
  	escape = 0;
  	signal( SIGINT, ctrlc );
  	S2Sdd_Init( &buf );
  	printf( "Buffer initialized\n" );

  	// URGのパラメータ取得
  	Scip2CMD_PP( port, &param );

  	// URG-04LXの全方向のデータを取得開始
  	Scip2CMD_StartMS( port, param.step_min, param.step_max,
                    	1, 0, 0, &buf, SCIP2_ENC_3BYTE );

  	while( !escape ){
    	ret = S2Sdd_Begin( &buf, &scan );
    	if( ret > 0 ){
      		int j;
      		// 新しいデータがあった時の処理をここで行う
      		printf( "Front distance: %lu mm\n", 
               		scan->data[ param.step_front - param.step_min ] );

      		// 処理例:スキャンしたデータをxy座標(m単位)に変換
      		for( j = 0; j < scan->size; j ++ ){
        		float x, y;
        		float scan_theta;

        		// scan->data[j]はmm単位の距離を表し、
        		// 20mm以下の距離は測距エラーを意味する
        		if( scan->data[j] < 20 ) continue;

       			scan_theta = M_PI * 2.0 * ( j - ( param.step_front - param.step_min ) ) / 
                        param.step_resolution;
        		// URGが上下逆についている場合は、scan_theta = -scan_theta;
        		x = scan->data[j] * 0.001 * cos( scan_theta );
        		y = scan->data[j] * 0.001 * sin( scan_theta );

        		// このx,yやscan->data[]の値を上手く使って処理を行う
      		}
      		// S2Sdd_BeginとS2Sdd_Endの間でのみ、構造体scanの中身にアクセス可能
      		S2Sdd_End( &buf );
    	}
    	else if( ret == -1 ){
      		// 致命的なエラー時(URGのケーブルが外れたときなど)
      		fprintf( stderr, "ERROR: Fatal error occurred.\n" );
      		break;
    	}
    	else{
      		// 新しいデータはまだ無い
      		usleep( 10000 );
    	}
  	}
  	printf( "\nStopping\n" );

  	ret = Scip2CMD_StopMS( port, &buf );
  	if( ret == 0 ){
    	fprintf( stderr, "ERROR: StopMS failed.\n" );
    	return 0;
  	}

  	printf( "Stopped\n" );
  	S2Sdd_Dest( &buf );
  	printf( "Buffer destructed\n" );
  	Scip2_Close( port );
  	printf( "Port closed\n" );

  	return 1;
}
		

このサンプルプログラムをコンパイルする際には、以下のようなコンパイルオプションと実行時の引数が必要です。

$ gcc test.c -o test -lscip2awd -lypspur -lpthread -lm
$ sudo chmod 777 /dev/ttyACM0
$ ./test /dev/ttyACM0
    
    

2014年の4月頃からコンパイルエラーが確認されるようになりました。 上記方法でエラーになる場合は以下のコンパイルオプションを試してみてください。

    
$gcc test.c -o test -Wl,--no-as-needed -lscip2awd -lypspur -lpthread -lm
        
    

また、上記のサンプルプログラムは、ポーリング型の処理を行っていますが、コールバック型の処理も可能です。 コールバック型のプログラムにしたい場合は、libscip2awd/sample/test-ms-callback.c のサンプルプログラムを参考にして下さい。

Failed to load shared libraryというエラーが出る場合は以下のコマンドを入力して下さい。

$ sudo su -
# echo "/usr/local/lib" > /etc/ld.so.conf.d/lib64.conf
# echo "/usr/local/lib64" >> /etc/ld.so.conf.d/lib64.conf
# ldconfig
# exit
		

課題

  1. ロボットを直進させ、前方1m以内に障害物を見つけたら停止させてください。
  2. ロボットを左側にある壁に沿って走らせてください。

課題のヒント

  1. 距離が20mm以下の値は、測距ができなかったというエラーを表しています。
  2. param構造体の、param.step_resolutionは角度の分解能を表しています。従って、step_front - step_min - step_resolution/4、step_front - step_min + step_resolution/4がそれぞれ90度右・左側を表します。
  3. センサデータが更新されるのは、S2Sdd_Begin関数を呼び出したタイミングです。S2Sdd_Begin関数を呼び出さないとデータは更新されません。S2Sdd_Beginを呼び出したら、必ずS2Sdd_Endが呼び出されている必要があります。
  4. センサの取り付けが上下逆になっているロボットもあるので見て確認して下さい。

課題2の考え方

ロボットに壁追従させる最も簡単な方法は、以下の図のようなアルゴリズムになります。

どうしてFSコマンドを使用するのか、少し考えてみましょう。

ロボットを壁追従させるのに、この方法は必ずしも良い方法ではありません。色々なアルゴリズムが考えられるので、余裕がある人はこれ以外の方法を考えてみましょう。

課題2ができたら

TAに見てもらってください。