2013b/Member/michaegon/Mission2 の履歴差分(No.3)

履歴一覧
現在との差分を表示
ソースを表示
2013b/Member/michaegon/Mission2 へ行く。
- 1 (2014-02-02 (日) 05:31:52)
- 2 (2014-02-02 (日) 05:58:39)
- 3 (2014-02-03 (月) 10:16:54)
- 4 (2014-02-05 (水) 13:47:05)
追加された行はこの色です。
削除された行はこの色です。
目次
#contents

*課題2内容 [#vb71dca6]
>2台のロボットで空き缶を収集し、ゴールに空き缶になるべく高く積み上げる
>
>基本ルール
>競技時間は審判が続行不能と判断するまで、あるいはリタイアするまで。
>図の左下の隅からスタートする。ロボットが紙からはみ出ない範囲で可能な限りに隅に配置する。
>空き缶を集めて図の「ゴール」と書かれた円弧と直線で囲まれる領域に空き缶を運ぶ
>なるべく高く空き缶を積み上げる
>運び終わった後、あるいは積み上げた後はロボットはゴールの領域から外に出ること。また空き缶と接触していたはいけない。
>開始の合図から５秒以内にスタートボタンを押す作業を完了すること。
>競技が終了するまで、ロボットに触ったり人間が遠隔で操作してはならない。
>途中でうまく動かなくなった場合、１回限り再スタートすることができる（再スタートの際に別プログラムで起動してよい）。

以上[[2013b/Mission2]]より引用

&aname(map);
コース図
#ref(robocon2013b.png);

*メンバー [#zab397a9]
[[dyuma>2013b/Member/dyuma/Mission2]]~
ロボット製作（+ロボット管理）・プログラミングとどちらもこなしている中間管理職。

[[pepo>2013b/Member/pepo/Mission2]]~
ロボットを組ませたら彼の右に出るものはいない。（ただし情報工学科）

michaegon~
このページの著者~
プログラムの方をやってます。

*ロボット概要 [#t8caacae]

*プログラム概要 [#u38c8111]
ここでは、このページの著者michaegonが作成したプログラムの概要を掲載します。
他メンバーが作成したプログラムについては他メンバーのページをご覧ください。
**PID_4.1.nxc(ライントレース用) [#bea8657d]
 #define KP 1.55 // 比例ゲイン
 #define KI 0.00005// 積分ゲイン
 #define KD 0.85// 微分ゲイン
 #define DT 1
 #define THRESHOLD 50 // 閾値
 #define SPEED1 40 // 高速
 #define SPEED2 25 // 低速
 #define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
 #define JUDGE_T 2500 // ギアチェンジ基準時間
↓今回のために作ったプログラムです。
ライントレース部分を他の人が作ってもちゃんと動作するようにファイルを分割してあります。
*** TIRE_main.nxc [#e1f19394]
タイヤを制御してライントレースをする方のメイン部分です。~
ここで使われている関数は、linetrace.nxc、tire.nxcで定義されています。

 // ライントレース用タイヤ制御側プログラム(MASTER)
 //main部分
 // ライントレース部分はlinetrace.nxcに書き込んでください
 
  float pid;
  int result;
  int dev[2]; // 偏差
  long time;
  int flag;
  float integral; 
 #include "tire.nxc"
 #include "linetrace.nxc"
 
  
 /*①*/
 inline int ope_amount(int sensor)
 { 
 
  dev[0]=dev[1]; // 過去のセンサー値と閾値の差
  dev[1]=sensor-THRESHOLD; // 現在のセンサー値と閾値の差
  if (dev[1]==dev[0]) { // もしセンサー値の変化がない場合はintegralをリセットする
     integral=0;
  }
  integral+=(dev[1]+dev[0])*DT/2.0; // 積分を計算する
  
  pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
  
  if (-100>pid) {
     result=-100;
  } else if (100<pid) {
     result=100;
  } else {
     result=pid;
  }
 
  return result; // -100から100の間でシンクロ率を返す
 }
 
 
 /*②*/
 task gearchange()
 {
  time=CurrentTick();
  until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
  if (CurrentTick()-time>=JUDGE_T) {
     flag=1;
  } else {
     flag=2;
  }
 }
 
 
 task main()
 {
  SetSensorLight(S1);
  SetSensorLowspeed(S2);
  
  // 低速版
  while (true) { // 左エッジ用
        if (flag==1) {
           stop gearchange;
           flag=0;
           PlaySound(SOUND_UP);
           break;
        } else if (flag==2) {
           stop gearchange;
           flag=0;
        } 
  // スタート地点からコースに乗るまでの行動
  start_action();
 
 
        if (SENSOR_1>THRESHOLD) {
           start gearchange;
        }
        Low:
        GO(SPEED2,ope_amount(SENSOR_1));
  }
  // 缶集めと缶建てそれぞれ2回ずつ
  phase1();
  can_build1();
  phase2();
  can_build2();
  
 // 高速用
  while (true) { // 左エッジ
        if (flag==1) {
           stop gearchange;
           flag=0;
           PlaySound(SOUND_DOWN);
           goto Low;
        } else if (flag==2) {
           stop gearchange;
           flag=0;
        }
  // コース外への離脱を定義
  end_action();
  
 
        if (SENSOR_1>THRESHOLD) {
           start gearchange;
        }
        GO(SPEED1,ope_amount(SENSOR_1));
  }
 }
※このプログラムは時計回りライントレース用です。
*** linetrace.nxc [#n8321ff4]
ライントレース部分を定義するファイルです。ここではPID制御でのライントレースのプログラムを書いておきます。

***経緯・基本方針 [#f7c7d47f]
PID_4.1.nxcはメンバーの[[dyuma>#zab397a9]]の、設定した閾値と光センサーの実測値の差によってロボットのカーブ量を決定してライントレースをするという考えがもとになっています。~
 /*
 閾値と光センサーの値の差によってカーブ量を決めるプログラム
 時計回りライントレース用
 */
 #define SPEED 30 // 前進スピード
 #define THRESHOLD 50 // 閾値
 #define turn_right(s) OnFwd(OUT_C,SPEED); OnRev(OUT_B,s); // 右に曲がる
 #define turn_left(s) OnFwd(OUT_B,SPEED); OnRev(OUT_C,s); // 左に曲がる

*** ARM_main.nxc [#gc20b1f4]
アーム制御をする方のメイン部分です。

 // アーム部main
 
 #include "arm.nxc"
 
 task main()
 {
  SetSensorLight(S1);
 
  int back; // 逆回転する方のタイヤのスピード
 
  int msg;
  while (true) {
    while (SENSOR_1<THRESHOLD) { // ロボットがライン上にいるとき
      back=THRESHOLD-SENSOR_1; // 閾値とセンサー実測値の差をスピードにする
      turn_right(back); // ライン上にいるときは右に曲がる
      Wait(1);
    }
    while (SENSOR_1>=THRESHOLD) { // ロボットがライン外にいるとき
      back=SENSOR_1-THRESHOLD;
      turn_left(back); // ライン外にいるときは左に曲がる
      Wait(1);
    }
        msg=0;
        // 蹴り上げを指令を確認
        ReceiveRemoteNumber(MAILBOX1,true,msg);
        if (msg) {
           msg=0;
           hit();
        }
        // 缶建ての指令を確認
        ReceiveRemoteNumber(MAILBOX3,true,msg);
        if (msg) {
           msg=0;
           build();
        }
  }
 }
このプログラムには、まっすぐ進めず走行がジグザグになり滑らかでない、前進スピードが遅いなどの欠点がありました。~
何か解決策はないか調べていたところ、見つけたのが"PID制御"です。~
>PID制御(ピーアイディーせいぎょ = Proportional Integral Derivative Controller)は、フィードバック制御の一種であり、入力値の制御を出力値と目標値との偏差、その積分、および微分の3つの要素によって行う方法のことである。
*** arm.nxc [#x92c7d8b]
アーム制御にかかわる関数を定義したファイルです。
*** stdfunc.h [#i7ccb149]
複数のプログラムに共通して使えそうなものを定義したヘッダファイルです。

「PID制御」『フリー百科事典　ウィキペディア日本語版』(http://ja.wikipedia.org/wiki/PID%E5%88%B6%E5%BE%A1 )2013年6月18日 (火) 17:14 UTC より引用~
 #define TIRE 0 // master
 #define ARM 1 // slave
 #define SPEED 32
 #define SPEED2 15
 #define KP 1.6
 #define KI 0.00002
 #define KD 0.8
 #define THRESHOLD 50
 #define THRESHOLD2 32
 #define RIGHT 1
 #define LEFT -1
 #define DT 1
 #define CT 250
 #define FIRST_T 5000
** ライントレース部分の仕様 [#bea8657d]
今回もPID制御を利用したライントレースプログラムを組んでみました。~
前回の反省を生かしてより安定して成功できるものに改良しました。
PID制御については[[前回>2013b/Member/michaegon/Mission1]]のページをご覧ください。
***交差点・急カーブ判定 [#dd382873]
基本的なライントレースのプログラムは以下の通りです。

今回は、操作量をope、変数x,y,z、定数Kp（比例ゲインという）、Ki（積分ゲイン）、Kd（微分ゲイン）を用いて以下の式をたてて利用します。&aname(pid);~
#ref(tex1.jpg)
PID_4.1.nxcでは、OnFwdSyncに用いるモーターの同期（シンクロ）率にopeを用います。

&ruby(Proportional){比例};(P)制御では、比例ゲインKpと、閾値と光センサーの実測値の差（偏差）xを用いて操作量を決定します。~
ロボットの現在位置が閾値（今回はラインの境界上）から離れれば離れるほど比例的に操作量が大きくなります。

&ruby(Integral){積分};(I)制御では、積分ゲインKiと、過去の光センサー実測値の累積yを用いてカーブ中などのロボットの位置を補正します。~
比例(P)制御だけでは、ロボットが閾値近辺を進んでいる時は操作量が小さくなり、ラインがカーブを描いていたりすると、カーブに対応できずにライン（閾値）から若干離れた位置で安定してしまうことがあります。そのようなときに、比例(P)制御では対応できないようなわずかな偏差をも蓄積していき、その量に応じて操作量を増やす積分(I)制御を使ってロボットをより閾値に近い位置に補正します。

&ruby(Derivative){積分};(D)制御では、微分ゲインKdと、光センサー実測値の変化（傾き）zを用いて、未来にカーブがあるかなどを予測して操作量を決定します。これによってカーブの始まりでラインから大きく外れてしまうという事態を防ぎます。

***KP,KI,KDの決定方法 [#h5ec5df1]
比例ゲインKP,積分ゲインKI,微分ゲインKDの最適な値を見つける方法には、有名なところでは限界感度法などがあります。~
今回は限界感度法を試してみました。

限界感度法では、まずはじめにKI=KD=0としてP制御だけで走行させてみます。~
このときKPの値を徐々に増やしていって、ロボットが持続的に振動する（sinカーブのような軌道を描きながら走行し続ける）状態になるKPを探します（これをKuとおく)。~
このときのロボットの振動周期をTuとおいて、Ku,TuからPID制御にしたときの適切なKP,KI,KDを算出します。

このようにうまく限界感度法が使えればよかったのですが、実際やってみるとうまくいくいませんでした。~
そもそもKPの値をいくら大きくしても振動が持続せずにすぐに振動が減衰してしまいKu,Tuがうまく出せなかったので、今回は限界感度法使うのを断念しました。

ということで今回は初めから手作業で値を決めました。~
PID制御の中でメインになるのはP制御で、ID制御はP制御の補助のようなものなので、まずはP制御のみで走行させます。この時、一番滑らかに安定して走行したKPを見つけて使用しました。~
次に、I制御を加えてカーブを走行させます。I制御では若干の位置補正ができればいいので、KIの値は、ロボットがP制御のみの時よりもカーブに吸い付くように走行して、あまり大きくない辺りに設定しておきます。~
最後のD制御は、急激なコース変化に対応させるためのもので、あまりKDの値を大きくしすぎると、コースのわずかな変化にも過剰に反応して、走行中にロボットが無駄な振動をして不安定な走行になってしまいます。よって、KDの値もまあり大きすぎない辺りに設定しておきます。

***① inline関数ope_amount [#k44c6886]
 /*PID_4.1.nxcの一部*/
 #define KP 1.55 // 比例ゲイン
 #define KI 0.00005// 積分ゲイン
 #define KD 0.85// 微分ゲイン
 #define SPEED 32
 #define KP 1.6
 #define KI 0.00002
 #define KD 0.8
 #define THRESHOLD 50
 #define THRESHOLD2 32
 #define RIGHT 1
 #define LEFT -1
 #define DT 1
 #define THRESHOLD 50 // 閾値
 #define CT 250
 
 float pid;
 int result;
 int dev[2];
 int dev[2]; // 偏差
 float integral;
 
 inline int ope_amount(int sensor)
 { 
 // 操作量を返す関数
 inline int ope_amount()
 {
  float pid;
  int result;
 
  dev[0]=dev[1]; // 過去の偏差
  dev[1]=sensor-THRESHOLD; // 現在の偏差
  if (dev[1]==dev[0]) { // もし偏差変化がない場合はintegralをリセットする
  dev[1]=SENSOR_1-THRESHOLD; // 現在の偏差
 
  if (dev[0]==dev[1]) { // 不要になったらintegralリセット
     integral=0;
  }
  integral+=(dev[1]+dev[0])*DT/2.0; // 積分を計算する
  
  pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT;
  
  if (-100>pid) {
     result=-100;
  } else if (100<pid) {
     result=100;
  } else {
     result=pid; // int型にキャスト
  integral+=(dev[1]+dev[0])*DT/2; // 積分計算
 
  pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT; // PID
 
  result=pid; // int型に変換
  /*シンクロ率の範囲におさめる*/
  if (result>SYNCMAX) {
     result=SYNCMAX;
  } else if (result<SYNCMIN) {
     result=SYNCMIN;
  }
 
  return result;
 }
KPは比例ゲイン、KIは積分ゲイン、KPは微分ゲイン、DTは光センサーの値を記録する間隔（今回は1ミリ秒）です。~
dev[0]には1ミリ秒前の光センサーの値と閾値の差（偏差）、dev[1]には現在の偏差を入れます。~
次はI制御のための準備です。integralには、1ミリ秒ごとに(dev[1]+dev[0])*DT/2（下図の黄色い部分）を足していきます（積分）。~
#ref(integral.jpg) 
このとき、integralに偏差が蓄積し続けると、I制御での補正が必要なくなっても操作量が無駄に増えたままになってしまうので、偏差変化がなくなったとき（直線上など）にはintegralをリセットする機能を付けてあります。

浮動小数点pidには、PID制御によって決定した操作量を入れます（[[式(1)>#pid]]）。~
ここで、~
Kp*dev[1]はP制御、~
Ki*integralはI制御、~
また、DTを微小時間とみなして光センサー値推移のグラフの傾きを(dev[1]-dev[0])/DTとしたとき（微分）、~
Kd*(dev[1]-dev[0])/DTはD制御による操作量を表しています。
#ref(d.jpg),left

上で求めたpidをOnFwdSyncのシンクロ率（範囲は-100から100までの整数）として使いたいので、この関数の最後のif文で、int型のresultにpidを代入して整数値に変換したあと、-100から100までに収まるように値を調整します。

***② メインタスク・サブタスクgearchange [#f48902aa]
 /*PID_4.1.nxcの一部*/
 #define THRESHOLD 50 // 閾値
 #define SPEED1 40 // 高速
 #define SPEED2 25 // 低速
 #define GO(s,t) OnFwdSync(OUT_BC,s,t); Wait(DT);
 #define JUDGE_T 2500 // ギアチェンジ基準時間
 int flag;
 
 task gearchange()
 /*
  黒線の、とある側を走行する関数（車線変更つき）
 */
 inline void go_xside(int sgn)
 {
  time=CurrentTick();
  until (SENSOR_1<=THRESHOLD || CurrentTick()-time>=JUDGE_T);
  if (CurrentTick()-time>=JUDGE_T) {
     flag=1;
  } else {
     flag=2;
  }
 }
 
 task main()
 {
  SetSensorLight(S1);
  
  // 低速走行ループ
  while (true) { // 左エッジ用
        if (flag==1) {
           stop gearchange;
           flag=0;
           PlaySound(SOUND_UP);
  while (true) {
        OnFwdSync(OUT_BC,SPEED,ope_amount()*sgn);
        Wait(DT);
        if (SENSOR_1<THRESHOLD2) {
           OnFwdSync(OUT_BC,SPEED,-ope_amount()*sgn);
           Wait(CT);
           break;
        } else if (flag==2) {
           stop gearchange;
           flag=0;
        } 
 
 
        if (SENSOR_1>THRESHOLD) {
           start gearchange;
        }
        Low:
        GO(SPEED2,ope_amount(SENSOR_1));
  }
  
 // 高速走行ループ
  while (true) { // 左エッジ
        if (flag==1) {
           stop gearchange;
           flag=0;
           PlaySound(SOUND_DOWN);
           goto Low;
        } else if (flag==2) {
           stop gearchange;
           flag=0;
        }
  
 
        if (SENSOR_1>THRESHOLD) {
           start gearchange;
        }
        GO(SPEED1,ope_amount(SENSOR_1));
  }
 }
もともとのメインタスクは、~
 
 task main()
 {
  while (true) {
     GO(SPEED1,ope_amount(SENSOR_1));
        go_xside(RIGHT);
        go_xside(LEFT);
  }
 }
だけでした。これでもライントレースはできるのですが、交差点やT字路を直進してしまい、曲がってくれませんでした。これは、交差点やT字路の直角部分に操作量の変化が追いつけず、まだ直線だと認識しているうちに通過してしまうことが原因のようでした。~
交差点を直線だと認識して通過してくれるのはありがたいのですが、T字路も直進してしまうとスタート地点から出られないので、T字路が現れる部分だけ速度を落としてカーブとして認識するように改良したのが今回のメインタスクです。
*実際に動かしてみた [#x7f1c2ad]

このプログラムを実行させると、まずは低速でライントレースをする無限ループに入ります。~
このループにいる限り、ロボットは交差点もT字路もカーブと判断して曲がっていきます。~
よって、初めのT字路は無事に曲がることができますが、このままではそのあとの交差点なども通過せずに曲がってしまいます。

さて、初めのT字路を、左に曲がった場合は次に直角カーブの外側を、右に曲がった場合は次にヘアピンカーブの外側を回ることになります。（[[コース図>#map]]）~
このとき、どうしてもロボットはラインよりも若干大回りになるため、２～３秒の間ずっと、光センサーの値が閾値よりも大きくなります。~
この現象は、比例ゲインなどを適切な値にしておけば、今回のコースの他の場所では起こりません。このことを利用して、速度変更を行います。

低速ライントレースのループ中で、光センサーの値が閾値より大きくなる（ロボットがライン外に出る）と、サブタスクgearchangeが作動します。~
サブタスクgearchangeは、以下の動作をします。
+光センサーの値が閾値より大きくなっている（ロボットがライン外にいる）時間を計測
+もし光センサーの値が閾値より大きかった時間が、~
JUDGE_T以上だった場合は、flagに1を代入~
JUDGE_T以下だった場合はflagに2を代入

すると、メインタスク内のif文が実行されます。このif文では、
+サブタスクgearchangeを停止させる
+flagを0に戻す
+gearchangeで~
flagに1が代入されていた（ロボットがJUDGE_T以上ライン外にいた）場合は低速ライントレースのループを抜けて高速ライントレースのループへ移動~
flagに2が代入されていた（ロボットがJUDGE_Tに達する前にライン上に戻った）場合はこのまま低速ライントレースのループを継続

この機能は高速ライントレースのループにも搭載されています。

ここで、ロボットがコースを時計回りにライントレースさせてみました。ロボットは、
+低速走行で始まり、初めのT字路を左に曲がる
+次の直角カーブの外側を回る~
このとき、JUDGE_T以上ラインから外れてロボットが回るので、ここで低速走行ループから高速走行ループに切り替わる
+高速走行中では、交差点やT字路を直線と判断して通過していくので、この後にある交差点は全て通過
+２つめの交差点を抜けてヘアピンカーブの外側を回る部分に差し掛かる~
カーブの曲がり具合に追いつけずJUDGE_T以上ロボットがラインから外れるので、高速走行から低速走行に切り替わる
+その後差し掛かるT字路は低速走行中なのでカーブ判定して曲がり、無事にゴールへ到達

このようにコースを一周できました。

ちなみに、
 GO(SPEED2,ope_amount(SENSOR_1));

 GO(SPEED1,ope_amount(SENSOR_1));
の部分を、
 GO(SPEED2,-ope_amount(SENSOR_1));

 GO(SPEED1,-ope_aount(SENSOR_1));
に変えるだけで反時計周りのライントレースもできました。

**can_move.nxc(缶移動関連) [#p5edb615]
 /*
 定義部
 */
 #define SPEED1 40
 #define SPIN RotateMotorEx(OUT_BC,100,180,100,true,true); Wait(2000);
 #define CATCH(s) RotateMotor(OUT_A,s,180); Wait(2000); Off(OUT_A);
 
 
 /*
 関数
 */
 inline void can_move()
 {
  Float(OUT_BC);
  Wait(100);
  Off(OUT_BC);
  
  CATCH(100); // 缶をつかんだ状態
  RotateMotorEx(OUT_BC,SPEED1,1080,0,true,true); // 若干前進,回転角は適宜変更を
  Wait(100);
  Off(OUT_BC);
  /*
  旋回
  回転角は適宜変更してください
  */
  SPIN;
  CATCH(-100);
  /*
  旋回
  回転角は適宜変更を
  */
  SPIN;
 }
 
 
 /*
 task main内組み込み用
 */
 task main()
 {
  SetSensorLowspeed(S2);
  
  if (SensorUS(S2)<=10) {
     can_move();
  }
 }
※実際にはこれをPID_4.1.nxcに組み込んで使用します。

***基本方針 [#naba760e]
ライントレース中に前方に缶があるとき、超音波センサーで測って距離が10cmになったときに缶を動かすinline関数can_moveが作動します。

***inline関数can_move [#mdaee0c0]
缶との距離が10cmになったらFloatで止まらせます。SPEED1が40程度なら、これで缶にぶつからない程度で近づくことができます。~
完全に止まったら、アームをおろして缶を抱え込みます。そのまま若干前進したあとその場で旋回して缶が元あった場所に缶をおいてアームをあげて缶を放します。~
その後にもう１度その場で旋回して前を向いて、本来のライントレースプログラムへ戻り、ライントレースを続行します。


*反省点・改善点 [#p4525d91]
PID制御に使うKP,KI,KDの最適な値を求めるのが難しかったです。この値がおかしいとライントレースの成功率が格段に落ちてしまうのも苦労しました。~
調べていると求め方があったのですが、なかなかその方法を使ってもうまくいきませんでした。~
結局手探りで値探しをすることになってしまってずいぶん時間を取られてしまいました。~
今後は１から手探りにならないような方法を探して、適切な値を入れて成功率を上げていきたいです。

チームとしては[[dyuma>#zab397a9]]が缶を動かすプログラムの実戦投入していたので缶をどかしてライントレースする課題は達成はできました。~
ただ、個人的には、PIDの方でKP,KI,KDの値探しに時間をかけすぎて自分で作ったcan_move.nxcを実戦投入できませんでした。可能なら実戦投入しておきたいです。