« Beauto ステップアップ講座5 前進成分と回転成分の足し算 | トップページ | 本講座のサンプルをVS-WRC003LVに移植する場合の注意点 »

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の冒頭にある図は制御理論で使われるブロック線図というものです。見た目が似ているかも知れませんが、フローチャートとは違います。本講座の意図はこの図に集約されているので、この図を使って次回説明する予定です。

|

« Beauto ステップアップ講座5 前進成分と回転成分の足し算 | トップページ | 本講座のサンプルをVS-WRC003LVに移植する場合の注意点 »

コメント

コメントを書く



(ウェブ上には掲載しません)




トラックバック

この記事のトラックバックURL:
http://app.cocolog-nifty.com/t/trackback/123969/49210611

この記事へのトラックバック一覧です: Beauto ステップアップ講座6 PID制御器を用いたライントレース:

« Beauto ステップアップ講座5 前進成分と回転成分の足し算 | トップページ | 本講座のサンプルをVS-WRC003LVに移植する場合の注意点 »