« 2010年7月 | トップページ | 2010年9月 »

2010年8月

2010年8月23日 (月)

Beauto ステップアップ講座6 PID制御器を用いたライントレース

第3回では、Beauto Builderのようなソフトを使ったフローチャート式プログラミングと本講座での考え方の違いについて述べました。

  1. 動作時間の指定はしない
  2. 条件判断ではなく実数の計算による

今回はこのうち2.についてです。

Beauto Builderのようなフローチャート式プログラミングツールでは「右センサーが一定以上の値で反応したら」等、YesかNoで表せる条件に従って分岐するのが制御プログラムの主要な構造となります。前回まではこれと同じ発想で、センサーから得た情報を3種類に場合分けして行動を選択していました。そのため、直線の線上を走り続けているときでも、ずれがある値より大きくなった瞬間に今までと大きく異なる動きをしようとします。このためロボットの動きがギクシャクしたものになってしまいます。微妙なずれを微妙に調整しながら滑らかに走行するのには向いていません。より多くの場合分けをすれば多少改善されるかもしれませんが、本質的には変わりません。

今回から扱うのは、連続な量を読み取り、そのまま連続的なモーター指令値に変換するという考え方です。用いる基本的な道具は条件判断ではなく、四則演算、特に乗算です。


void line_trace(int speed, float Kp, float Kd)
{
	float prev_line_pos = 0.0;  //前回のラインの位置
	while(1) {
		// 正規化したセンサ値の計算、ライン位置の計算などは省略
		// ・・・(中略)・・・
		line_pos = ・・・;
		
		run(speed, Kp * line_pos + Kd * (line_pos - prev_line_pos)); // (1)

		// ・・・(中略)・・・

		Sync();
		prev_line_pos = line_pos;        // 前回ライン位置として記憶する (2)
	}

(1)がライントレースの制御をおこなっている部分です。場合分けはありません。順番に見ていきましょう。

runへの第1引数、前進速度は一定でspeedにしています。これによりロボットは常に前へ進みます。

第2引数の第1項Kp * line_posは、ラインからのずれに比例したちからで旋回させるためのものです。ラインから大きくずれればずれるほど速く回転して中央に戻ろうとします。なお、ライン位置に掛かる比例定数Kpは適当に調整する必要があります。本講座では、ラインが左に見えるときline_posは負、runの第2引数は左回転(反時計回り)させたいとき正と決めました。そのためKpは負の数でなければなりません。正にするとラインから逃げるロボットになります(是非一度試してみてください)。

第2項についてですが、まずprev_line_posはこのwhileループについて1回前の繰り返しのときのline_posを記憶しています。(2)がそれを行っています。このため(line_pos - prev_line_pos)は、「前回からラインの位置がどれだけ変化したか」、つまりロボットから見てラインが移動する速度になります。これを用いて第2項のKd * (line_pos - prev_line_pos)はラインが移動する速度に比例したちからでロボットを旋回させようとします。ロボットが右回転している時に左回転の力を加えることになり、左右に振れるのを抑えるように働きます。また、ラインに対して平行でないと平行にする方向の力を発生しますので、折れ線や急なコーナーでロボットが線に対して斜めになったときにも効果を発揮します。このKdも同様に負でなければなりません。

最後に、2つの項の効果が両方とも出るように足し算します。

今回説明した制御方式だけでは急すぎるカーブや直角コーナーには対応できません。すべてのセンサーが線から外れ、線を見失ってしまったときにはライン位置が正しく計算できないためです。急なカーブに対してはKpを大きくするか、あるいはセンサー数を増やして検知範囲を広くすることで線を見失いにくくするという対処が考えられます。しかし直角コーナーは無理です。その他、何かの拍子に線から外れた場合も考慮して何らかの対策を別途記述しておく必要があります。

一例として、線から外れたら線のあると思われる方向にもっと強く旋回するという方法があります。これを実現するには、線がどちらにあるか、もっと正確には「もし次の瞬間に線から外れたとしたらどちらに外れた可能性が高いか」を常に判断し、記憶しておく必要があります。センサーが3個以上あれば比較的簡単に実現できます。センサーが2個だと検知範囲が狭いので、確実性の面で少し難しくなります。

サンプルプログラム
使い方・変更点

基本的な使い方は前回までと同じです。パラメータがspeed、Kp、Kdの3つに変更されています。deadband、turn_speedは今回の制御方式では使わないのでなくしました。

すべてのセンサーが線から外れた時を考慮した処理の一例として、停止して走行を終了するようにしました。センサー数を変更する場合はこの部分も変更する必要があります。このままではちょっと急なカーブでもすぐコースアウトして止まってしまいます。このような部分は「条件判断して処理を選ぶ」という点でBeauto Builderを使うフローチャート式プログラミングと同じですので、適宜工夫してみてください。


  (line_trace関数内)

		if(sval[0] < 0.3 && sval[1] < 0.3) { //どのセンサーも白い床の上にあるとき
			break;  //ライントレース終了
		}

また、USBケーブルを外してからスタートできるように、基板上のスイッチをスタートボタンにしました。設定画面で何も入力せず[Enter]を押すとReady.と表示されます。この後スイッチを押すと走り出します。


(main関数内)
		//何も入力せずEnterを押すと設定モード終了
		SciStrTx("Ready,\r\n", 8);
		//ボタンを押すとスタート
		while(!getSW());

USBケーブルを抜き差しした場合は、再度通信できるようにするためにターミナルソフトで切断と接続の操作を行ってください。

走行の様子

電源4.8V、Aギア(減速比12.7:1)という条件で試したところではspeed=80くらいまではコースアウトせず走れました。直線ではあまり揺れずにまっすぐ走り、カーブも止まらず滑らかに回れているということで、第1回で掲げた目標は達成できたのではないかと思います。

次回予告

今回説明した制御方式は一般にPID制御と呼ばれるものの仲間に入ります。PID制御からIをなくしたものなので特にPD制御と呼ぶこともあります。次回、このPID制御と本講座の内容の関係について解説します。なぜこのような制御方式をするのか、またなぜこの制御方式でうまくいくのかについて解説したいと思っています。

なおPID制御に関する日本語版Wikipediaの記事はこの記事執筆時点でいくつか曖昧で不適切な点がありそのせいで分かりにくいものになっています。もし見るなら英語版をお勧めします。ついでですが、英語版PID Controllerの冒頭にある図は制御理論で使われるブロック線図というものです。見た目が似ているかも知れませんが、フローチャートとは違います。本講座の意図はこの図に集約されているので、この図を使って次回説明する予定です。

| | コメント (0) | トラックバック (0)

2010年8月21日 (土)

Beauto ステップアップ講座5 前進成分と回転成分の足し算

前回まででON-OFF制御器を作成し、その挙動をパラメータによって調整できるようにしました。ラインを捉えることはできるものの、前に進んでいませんでした。今回から前に進むようにします。

前進しながら左右方向に旋回もするという動作をわかりやすく記述するため、今回からMtr_Run関数に左右の車輪の速度(モーターのduty比)を明示的に書くのではなく、「前進速度」と「回転速度」とに分けて指定するようにします。サンプルプログラムではrunという関数がこれを行います。


void run(int speed, int turn_speed)
{
	int d1 = speed - turn_speed; // (1)
int d2 = speed + turn_speed;

// 絶対値が127(100%)を超えないようにする (2)
// duty比が0のとき、Mtr_Runに渡す数は0x80にしてブレーキ動作にする
if(d1 > 127) d1 = 127;
else if(d1 < -127) d1 = -127;
else if(d1 == 0) d1 = 0x80; // brake   
(3)
if(d2 > 127) d2 = 127;
else if(d2 < -127) d2 = -127;
else if(d2 == 0) d2 = 0x80; // brake

Mtr_Run(-d1, d2, 0, 0);     // (4)
}

(1)の部分で左右の車輪の速度を計算します。

  • 左の車輪の速度=(前進速度)-(回転速度)
  • 右の車輪の速度=(前進速度)+(回転速度)

例えば前進速度80、回転速度30の時は左右の車輪の速度は上の計算式よりそれぞれ50、110となります。こうすると左に旋回しながら前進することになります。

(4)では実際にMtr_Run関数を呼び出してモーターの出力を変えます。左右の車輪でモーターは逆向きになるので1番目の引数は-d1として回転方向を逆、つまりd1が正のとき左モーターがロボットを前進させる方向に回るようにしています。このあたりはロボットの機械設計と電気配線に合わせて変更する必要があります。
※とはいえ実は旋回速度や前進速度に負の値を設定することでつじつまあわせができてしまいますが。
run(80, 30)とすると最終的に、Mtr_Run(-50, 110, 0, 0)を実行することになります。

(2)は、speed=100、turn_speed=100のような大きな値を指定した時にd1やd2の値が127を超えてしまう場合、127で頭打ちにするための処理です。マイナス方向も同様です。頭打ちにした場合、厳密には回転速度、前進速度ともに希望した値になりません。ゆっくり走る場合は影響しませんが、留意しておいてください。

(3)はMtr_Runの仕様上の特別処理です。第3回 の余談として書いたことですが、Mtr_Runの仕様では0はフリー、0x80はブレーキとなっているのでd1やd2の計算結果が0になったときは0x80に直します。

このrun関数を使うとON-OFF制御によるライントレース部分はこのように書けます。なお、speedという変数を追加してあります。旋回も直進も第1引数が共通になっていることに注目してください。

	if(line_pos < -deadband) {        // ラインが左側にあるとき
run(speed, turn_speed);       // 左回転(反時計回り)しながら進む
} else if(deadband < line_pos) {  // ラインが右側にあるとき
run(speed, -turn_speed);      // 右回転(時計回り)しながら進む
} else {                          // ラインが大体真ん中にあるとき
run(speed, 0);                // まっすぐ進む
}

サンプルプログラム

使い方は前回とほぼ同じです。パラメータ調整画面に前進速度speedを追加し、また上の説明を反映してline_trace関数の仕様を変更しました。第3引数に前進速度speedを指定するようになっています。

パラメータを変えて挙動の違いを見てください。Aギア(減速比12.7:1)、電源4.8Vの条件で試したところではspeedが30くらいまではうまく走ることができました。

ラインから外れた場合の処理は入っていません。コースからある程度以上外れると大抵は直進して暴走しますので気をつけてください。本来は線から外れたら停止するか、線に復帰するようにすべきです。本講座でも余裕があれば後からいくつかの方法を説明します。

次回はPID制御の考え方を取り入れたライントレースについて説明します。

| | コメント (0) | トラックバック (0)

Beauto ステップアップ講座4 対話形式パラメータ調整画面の作成

パラメータといえば数学では「媒介変数」というものですが、ロボットのプログラミングではロボットの挙動を間接的に決めるための数値、という意味合いです。たとえば前回の「許容するライン中央からのずれ幅」や「ロボットを回転させる速度」もパラメータといえます。

最適なパラメータはロボットの設計、更には部品の個体差、コースの状況によって違います。今回は対話形式でのパラメータ調整を行う方法を紹介します。

今回はロボットのプログラミングというよりは次回以降の準備ですので、先にプログラムを示した上で必要な部分を説明します。

使い方

ビルド方法は第1回を参考にしてください。

まず前回同様、センサー調整を行います。センサー全部を黒い線の上に載せてスペースキーを押し、続いてセンサー全部を白い床の上に移動させてスペースキーを押します。

するとパラメータ調整画面が表示されます。


1. deadband=0.200000
2. turn speed=127
>

これは変更するパラメータを選ぶ画面です。現在のパラメータの値を表示しています。deadbandは中央からどのくらいずれたら復帰しようとするか、turn speedは復帰動作で車体を回転させる速度です。たとえば線から外れた時の回転速度を変えたい場合は2[Enter]と操作します。すると

value=

と表示されますので変更後の値を入力してください。たとえば60[enter]と入力します。パラメータ一覧に戻ります。変更されたのを確認してください。

パラメータを選ぶ画面、「>」に対して[Enter]のみを入力すると調整モードを終了します。

調整モード終了後、スペースキーを押すとライン追従動作を開始します。ロボットをラインの上に置いて動作を確認してください。[Esc]キーを押すとライン追従動作を終了し、パラメータ調整画面に戻ります。

パラメータによってロボットの動きが変わるのを確認してください。

解説

ライン追従のプログラムはline_traceという名前の関数にまとめました。中のプログラムは前回とほとんど同じです。違いはturn_speed=回転速度とdeadband不感帯幅を引数として受け取り、それに応じた動作をする点です。例えば車体を回転させるようにモーターを駆動する部分は次のようになっています。

Mtr_Run(turn_speed, turn_speed, 0, 0); // 車体を左回転させる

パラメータ設定のメニューに相当する部分です。


while(1) {
int i;
char buf[80];
// 設定メニューを表示する
sprintf(buf, "1. deadband=%f\r\n2. turn speed=%d\r\n>",
        deadband, turn_speed);
SciStrTx(buf, strlen(buf));

// どの値を調整するか入力する
if(inputDecInt(&i) == 0) {
//何も入力せずEnterを押した場合
break;
} else {
//何かが選択された場合
SciStrTx("value=", 6);
switch(i) {
case 1:
inputDecFloat(&deadband);
break;
case 2:
inputDecInt(&turn_speed);
break;
}
}
}

シリアル通信から数値を入力できるようにするため、inputDecIntinputDecFloatという関数を用意しました。

  inputDecInt
  シリアル通信から整数を入力する
 
  引数:
  int *dst    結果を格納する変数のアドレス
 
  戻り値:
  正常に1つの整数を読み取れたら1、
  そうでないとき(たとえば何も入れずEnter)は0。

inputDecFloat(&deadband);のように使います。呼び出されると(シリアル通信の)入力待ちになります。Enterを押すと変数deadbandに入力した数が入ります。1文字も数字がなかった場合、例えばabc[Enter]と入力した場合や[Enter]のみを入力した場合はdeadbandの値は変わりません。

&はアドレス演算子というものです。詳しくは大抵のC言語の入門書の関数呼び出しおよびポインタに関する項に載っていますので割愛します。

| | コメント (0) | トラックバック (0)

« 2010年7月 | トップページ | 2010年9月 »