2013b/Member/michaegon/Mission1 の変更点 - ロボティクス入門ゼミ

追加された行はこの色です。
削除された行はこの色です。
2013b/Member/michaegon/Mission1 へ行く。
2013b/Member/michaegon/Mission1 の差分を削除
目次
#contents

*課題1内容 [#vb71dca6]
>自作したコースに沿って一周するするロボットを作成せよ。
>
>ただし、次の点に注意すること
>
>一人は時計回り用、もう一人は半時計回うり用のプログラムを作成すること（ロボット本体は同一のものを使う）。
>次のステップでプログラムを作成すること。
>交差点をきちんと認識し、正しく判断して通過できるようにする。
>確実に一周できるようにする。スタート地点から出発して元にもどるようにする。
>空き缶を交差点から10cm以上離した線上に起き、それを一旦どけて再び元の位置にもどすように改良する。
>1個の空き缶でうまくいったら、2個の空き缶でやってみる。 

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

&aname(map);
コース図
#ref(coi.jpg);

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

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

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

*ロボット概要 [#t8caacae]
今回の課題にあたってチームで使ったロボットの前面には、ライントレース用の光センサーと、缶認知用の超音波センサーが1つずつついています。
**アーム [#caac7b24]
#ref(robot.jpg);
今回使ったアームは、物をつかむよりは確実性があるだろうということで、物を抱え込む形にしました。~
普段のアームは上図のようにおりている状態ではなく、上がった状態で待機しています。物を抱え込むときには上図のようにおりてきます。

**光センサー [#wd2ba5d2]
#ref(light.jpg);
ライントレース用の光センサーは設置位置に気を配りました。~
説明書に書いてある通りに作ったロボットだと、光センサーの位置がタイヤの軸から遠いため、急カーブを走行するときなどに、光センサー自体はカーブ上にあってもタイヤ（本体）がコースアウトしてしまうことがありました。~
今回はタイヤの軸に極力光センサーを近づけて、ロボットの構造が原因でのコースアウトが起こらないように気を付けました。

以下では右タイヤをモーターB、左タイヤをモーターC、アームをモーターA、光センサーをセンサー1、超音波センサーをセンサー2に接続しているものとして説明していきます。
*プログラム概要 [#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 // ギアチェンジ基準時間
 
  float pid;
  int result;
  int dev[2]; // 偏差
  long time;
  int flag;
  float integral; 
 
  
 /*①*/
 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);
  
  // 低速版
  while (true) { // 左エッジ用
        if (flag==1) {
           stop gearchange;
           flag=0;
           PlaySound(SOUND_UP);
           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));
  }
 }
※このプログラムは時計回りライントレース用です。

***経緯・基本方針 [#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); // 左に曲がる
 
 task main()
 {
  SetSensorLight(S1);
 
  int back; // 逆回転する方のタイヤのスピード
 
  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);
    }
  }
 }
このプログラムには、まっすぐ進めず走行がジグザグになり滑らかでない、前進スピードが遅いなどの欠点がありました。~
何か解決策はないか調べていたところ、見つけたのが"PID制御"です。~
>PID制御(ピーアイディーせいぎょ = Proportional Integral Derivative Controller)は、フィードバック制御の一種であり、入力値の制御を出力値と目標値との偏差、その積分、および微分の3つの要素によって行う方法のことである。

「PID制御」『フリー百科事典　ウィキペディア日本語版』(http://ja.wikipedia.org/wiki/PID%E5%88%B6%E5%BE%A1 )2013年6月18日 (火) 17:14 UTC より引用~

今回は、操作量を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 DT 1
 #define THRESHOLD 50 // 閾値
 
 float pid;
 int result;
 int dev[2];
 float integral;
 
 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; // int型にキャスト
  }
 
  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()
 {
  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);
           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));
  }
 }
だけでした。これでもライントレースはできるのですが、交差点やT字路を直進してしまい、曲がってくれませんでした。これは、交差点やT字路の直角部分に操作量の変化が追いつけず、まだ直線だと認識しているうちに通過してしまうことが原因のようでした。~
交差点を直線だと認識して通過してくれるのはありがたいのですが、T字路も直進してしまうとスタート地点から出られないので、T字路が現れる部分だけ速度を落としてカーブとして認識するように改良したのが今回のメインタスクです。

このプログラムを実行させると、まずは低速でライントレースをする無限ループに入ります。~
このループにいる限り、ロボットは交差点も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を実戦投入できませんでした。可能なら実戦投入しておきたいです。