目次

走行パターン生成

 MCRのコースは、直線、曲線、クランク、レーンチェンジで
 構成されています。



 画像データよりコースを把握して、左右のモータに与える
 Duty比を決め、内蔵モジュールに設定すると、マシンは動
 きます。

 今回は、コースの状態を次の3つに分類しました。

 c328から入力される画像データを解析するために
 物理的特徴から、少しだけ計算を加え、3分類に
 集約しました。

 画像データの視野計算から、NORMAL、CRANK、LANE_CHANGE
 で処理方法を変えればよいと判断できます。

 視野計算と3つの走行パターン生成について説明します。


画像データの視野計算

 制御ボードを外したマシンが、次の写真です。  カメラは、水平面に対して45°傾けます。  水平のままでは、遠方の状態はよく分かりますが  手前は、ごく一部だけが見えるだけになります。  走行に必要な情報を取得するため、45°傾けます。  カメラは、視野角が57°なので、45°の斜面から  最大で118.5°、最小で61.5°まで見えます。  また、コース面からカメラの中心までの高さを求めると  45/sqrt(2)=32mmとなります。  さらに、カメラは直角2等辺3角形の頂点にあるので  取付け金具より32mmはなれます。  これらの物理諸元から、画像の上と下で1ラインに  含まれる情報を算出するTcl/Tkのスクリプトを作成  して検討します。  視野角を0〜57°まで1°単位で変化させます。  その視野角で、カメラから見えるラインを上から下まで  計算します。最後に1ライン=80ピクセルで、何mmの幅が  見えるかを計算します。 ------------------------------------------------------------------------- # calculate pi set pi [expr 4 * atan(1) ] # loop set i 0 while { $i < 58 } { # calculate set j [expr 90 + 28.5 - $i] set k [expr 135 - $j] set r [expr (32 * sqrt(2) / 2) / tan($k * $pi / 180)] set l [expr $r * 2 / tan(28.5 * $pi / 180)] set p [expr $l / 80] # generate code set pgm [format "%3d %4.1f %4.1f %4.1f %4.1f %.1f" $i $j $k $r $l $p] # show puts $pgm # increment incr i } -------------------------------------------------------------------------  上の計算で、カメラからの仰角、コース面からの仰角、  焦点からの水平距離、1ラインで見える横幅、1ピクセル  で見える最小幅を求めました。 ================================ 仰角 仰角 距離 横幅 ピクセル-幅 0 118.5 16.5 107.4 395.7 4.9 1 117.5 17.5 100.9 371.7 4.6 2 116.5 18.5 95.1 350.3 4.4 3 115.5 19.5 89.9 331.0 4.1 4 114.5 20.5 85.1 313.5 3.9 5 113.5 21.5 80.8 297.6 3.7 6 112.5 22.5 76.8 283.0 3.5 7 111.5 23.5 73.2 269.6 3.4 8 110.5 24.5 69.8 257.2 3.2 9 109.5 25.5 66.7 245.7 3.1 10 108.5 26.5 63.8 235.1 2.9 11 107.5 27.5 61.1 225.2 2.8 12 106.5 28.5 58.6 215.9 2.7 13 105.5 29.5 56.2 207.2 2.6 14 104.5 30.5 54.0 199.0 2.5 15 103.5 31.5 51.9 191.3 2.4 16 102.5 32.5 49.9 184.0 2.3 17 101.5 33.5 48.1 177.1 2.2 18 100.5 34.5 46.3 170.5 2.1 19 99.5 35.5 44.6 164.3 2.1 20 98.5 36.5 43.0 158.4 2.0 21 97.5 37.5 41.5 152.8 1.9 22 96.5 38.5 40.0 147.4 1.8 23 95.5 39.5 38.6 142.2 1.8 24 94.5 40.5 37.3 137.2 1.7 25 93.5 41.5 36.0 132.5 1.7 26 92.5 42.5 34.7 127.9 1.6 27 91.5 43.5 33.5 123.5 1.5 28 90.5 44.5 32.4 119.3 1.5 29 89.5 45.5 31.3 115.2 1.4 30 88.5 46.5 30.2 111.2 1.4 31 87.5 47.5 29.2 107.4 1.3 32 86.5 48.5 28.2 103.7 1.3 33 85.5 49.5 27.2 100.1 1.3 34 84.5 50.5 26.2 96.6 1.2 35 83.5 51.5 25.3 93.2 1.2 36 82.5 52.5 24.4 89.9 1.1 37 81.5 53.5 23.5 86.7 1.1 38 80.5 54.5 22.7 83.6 1.0 39 79.5 55.5 21.9 80.6 1.0 40 78.5 56.5 21.1 77.6 1.0 41 77.5 57.5 20.3 74.7 0.9 42 76.5 58.5 19.5 71.8 0.9 43 75.5 59.5 18.7 69.0 0.9 44 74.5 60.5 18.0 66.3 0.8 45 73.5 61.5 17.3 63.6 0.8 46 72.5 62.5 16.6 61.0 0.8 47 71.5 63.5 15.9 58.4 0.7 48 70.5 64.5 15.2 55.9 0.7 49 69.5 65.5 14.5 53.4 0.7 50 68.5 66.5 13.8 51.0 0.6 51 67.5 67.5 13.2 48.5 0.6 52 66.5 68.5 12.5 46.2 0.6 53 65.5 69.5 11.9 43.8 0.5 54 64.5 70.5 11.3 41.5 0.5 55 63.5 71.5 10.6 39.2 0.5 56 62.5 72.5 10.0 37.0 0.5 57 61.5 73.5 9.4 34.7 0.4 ================================  この計算結果から得られる情報を解析します。

情報解析

 MCRのコースは、下図のように規定されています。  マシンがコース上にあるかは、両端の30mm白線を  検出できるか否かで判断できます。  計算で求めた内容を眺めてみます。 ================================ 仰角 仰角 距離 横幅 ピクセル-幅 0 118.5 16.5 107.4 395.7 4.9 1 117.5 17.5 100.9 371.7 4.6 2 116.5 18.5 95.1 350.3 4.4 3 115.5 19.5 89.9 331.0 4.1 4 114.5 20.5 85.1 313.5 3.9 * 5 113.5 21.5 80.8 297.6 3.7 * 6 112.5 22.5 76.8 283.0 3.5 ================================  仰角で、118.5°〜112.5°とするとコース幅いっぱい  で見ることができます。  仰角とライン番号は、完全に一致しませんが、4か5ライン目  のデータを取り出すと、両端の30mm白線を検出できそうです。  レーンチェンジでは、両端の白線を検出できないので  レーンチェンジ中だと判断できます。   300mm幅でみている分には、左右の端に同時には白線を   検出できません。  レーンチェンジモードに入るかどうかは、コース上にマーカが  あるので、それを使い判断します。  マーカを検出するには、1ピクセルが1mm程度で見えるとよい  でしょう。1ピクセルが1mm程度に見える部分は、以下です。 ================================ 仰角 仰角 距離 横幅 ピクセル-幅 36 82.5 52.5 24.4 89.9 1.1 37 81.5 53.5 23.5 86.7 1.1 38 80.5 54.5 22.7 83.6 1.0 39 79.5 55.5 21.9 80.6 1.0 * 40 78.5 56.5 21.1 77.6 1.0 41 77.5 57.5 20.3 74.7 0.9 42 76.5 58.5 19.5 71.8 0.9 43 75.5 59.5 18.7 69.0 0.9 ================================  中央の白線が20mmの幅で、さらに両側に10mm灰色の領域  があるので、少なくとも40mmを超えていないと、中央の  白線を検出できません。  40mmの約2倍の80.6mmで見える39ライン目を使います。  この位置では、カメラの中心は、支点から54.9mm離れます。  ここまでで、レーンチェンジモードでの処理を確定できました。  次にクランクモードの判定と走行方法を検討します。  クランクの前には、マーカとして白線が2本引かれています。  どちらに進めばよいか、この白線からは判定できません。  少なくともクランクに入るので、徐行すればよいでしょう。  クランクで左あるいは右に90°回頭する方法を考えます。  コース幅が300mmであることから、徐行しながら両端の状態を  捕捉していきます。  左か右で、端の白線を検出できなくなれば、白線がない方に  回頭します。  マシンの無限軌道を動かすホイールの大きさは、固定なので  45°回頭の時間は、ほぼ固定になります。この処理により  進行方向を決めておき、中央の白線を検出するまで、徐々に  回頭します。中央で白線を検出したとき、クランクパスが終了  します。  クランクの判断とパス方法が確定したので、直線、曲線を  走行する方法を考えてみます。  直線、曲線を走行するときは、39ライン目で白線の位置が  中央、左、右のどこにあるのかを検出します。  検出結果から、白線の位置が中央に来るように、モータを回  します。  直線、曲線を走行しているときに、クランクやレーンチェンジ  のためのマーカを検出すれば、対応するモードに遷移します。  方針が決まったので、状態遷移図を作成します。

状態遷移図作成

 走行中の状態は、NORMAL、CRANK、LANE_CHANGEなので  この3状態を遷移するときの条件を図示します。  画像の上で300mmのコース幅を見るときをLongRangeとし  39ライン目でコースを見るときをShortRnageとします。  NORMAL→CRANKは、LongRangeで両端の白を検出し、  ShortRangeで、すべて白を検出したときになります。  NORMAL←CRANKは、ShortRangeで中央に白を検出した  場合の遷移です。  NORMAL→LANE_CHANGEは、LongRangeで両端の白を検出し、  ShortRangeで、片側すべて白を検出したときになります。  NORMAL←LANE_CHANGEは、ShortRangeで中央に白を検出  した場合の遷移です。  3つの状態の遷移方法を仕様で決めたので、各状態での  走行を検討します。

直線、曲線走行

 直線、曲線走行は、NORMAL状態での走行です。  ShortRangeで検出した状態で左右のモータに与える  Duty比を決めます。
  1. 中央に白がある    左右のDuty比を同一に設定
  2. 中央から右に白がある 左Duty>右Dutyに設定
  3. 中央から左に白がある 左Duty<右Dutyに設定
  4. すべて白である    左右のDuty比を同一に設定(NORMAL→CRANK)
  5. 中央から右がすべて白 左右のDuty比を同一に設定(NORMAL→LANE_CHANGE)
  6. 中央から左がすべて白 左右のDuty比を同一に設定(NORMAL→LANE_CHANGE)
 曲線走行では、白が中央から左右にどれだけ離れているかを  判断し、Duty比の値を増減させます。  NORMAL→CRANKあるいはNORMAL→LANE_CHANGEでは、徐行スピード  で直進します。

クランク走行

 クランクでは、5つの状態に分けて走行します。
  1. LongRangeで、左か右の端に白がなくなるまで直進
  2. 左か右の端に白がない側に45°回頭
  3. ShortRangeで、中央に白を検出するまで直進
  4. 45°回頭
  5. 指定時間後、CRANK→NORMALと状態遷移
 左か右の端に白がない側に45°回頭するのは、確実に  中央の白を検出できるようにするためです。  45°回頭するために必要になる時間は、実際に走行させて  決めます。

レーンチェンジ走行

 レーンチェンジ走行では、7つの状態に分けて走行します。
  1. LongRangeで、左か右の端に白がなくなるまで直進
  2. 左か右の端に白がない側に45°回頭
  3. 指定時間直進
  4. ShortRangeで、中央に白を検出するまで直進
  5. 45°回頭し、マシンを進行方向にあわせる
  6. 指定時間直進
  7. CRANK→NORMALと状態遷移
 3、6で必要となる時間は、実際に走行させて決めます。

画像処理

 画像処理は、80ピクセルを80ビットに変換する2値化とします。  最初に白と黒を判定するための閾値を求めておきます。  走行前に、4回画像を取得して、白と黒の平均を求めます。  4回とするのは、利用したSRAMが32kバイトで、80x60の画像  データを4枚まで保存できるからです。  上で求めた値を利用して、画像の2値化のための閾値を求めて  おき、次の処理を実行します。  1ピクセルは8ビットなので、0〜255までの数値をとります。  80ピクセルの最大値、最小値を求めて、相加平均を計算します。  相加平均を下回った値を0、それ以外を1にします。  相加平均が、先に求めた閾値より小さい場合は、閾値を利用して  0と1を決めます。  2値化のシーケンスは、以下とします。
  1. 0ピクセルのデータを、仮の最大、最小とする
  2. Mを1とする
  3. Mピクセルのデータが、最大より大きい場合、そのデータを最大とする
  4. Mピクセルのデータが、最小より小さい場合、そのデータを最小とする
  5. Mの値を1増やす
  6. M<80のとき、2に戻る
  7. 最大、最小の相加平均を求める
  8. Mを0とする
  9. Mピクセルのデータが、相加平均より小さい場合、0とする
  10. Mピクセルのデータが、相加平均以上の場合、1とする
  11. Mの値を1増やす
  12. M<80のとき、9に戻る
 移動しながら、80ビットをスキャンしパターン認識するには時間が  かかりすぎるので、80ビットを8ビットにする方法を考えます。  80ビットを、10ビットずつにして8ブロックにします。  各ブロックの代表値を求め、その値を指定値と比較し、各ブロック  の0と1を決めます。  各ブロックの代表値は、10ビットの加算値とします。  また指定値は、各ブロックの代表値の最大、最小の相加平均とします。  LongRange、ShortRangeの2ラインの処理が必要となるので  関数にまとめます。  UBYTE get_road(UBYTE *x)  {   UBYTE gdat[80];   UBYTE cat[8] ;   UBYTE result ;   UBYTE res0 ;   UBYTE smax ;   UBYTE smin ;   UBYTE i ;   UWORD avr ;   /* calculate maximum and minimum */   for ( i = 0 ; i < 80 ; i++ ) {   /* set initial values */   *(gdat+i) = 0 ;   if ( i == 0 ) { smax = smin = *x ; }   /* compare */   if ( smax < *(x+i) ) { smax = *(x+i) ; }   if ( smin > *(x+i) ) { smin = *(x+i) ; }   }   /* average */   avr = (smax + smin) >> 1 ;   /* store */   for ( i = 0 ; i < 80 ; i++ ) {   if ( *(x+i) > avr ) { *(gdat+i) = 1 ; }   }   /* smax = smin */   res0 = 0 ;   if ( smax == smin ) { res0 = smax ; }   /* calculate block sum */   *(cat+0) = *(cat+1) = *(cat+2) = *(cat+3) = 0 ;   *(cat+4) = *(cat+5) = *(cat+6) = *(cat+7) = 0 ;   for ( i = 0 ; i < 10 ; i++ ) {   *(cat+0) += *(gdat+i+10*0); *(cat+1) += *(gdat+i+10*1);   *(cat+2) += *(gdat+i+10*2); *(cat+3) += *(gdat+i+10*3);   *(cat+4) += *(gdat+i+10*4); *(cat+5) += *(gdat+i+10*5);   *(cat+6) += *(gdat+i+10*6); *(cat+7) += *(gdat+i+10*7);   }   /* calculate block line maximum and minimum */   for ( i = 0 ; i < 8 ; i++ ) {   if ( i == 0 ) { smax = smin = *(cat+0) ; }   if ( smax < *(cat+i) ) { smax = *(cat+i) ; }   if ( smin > *(cat+i) ) { smin = *(cat+i) ; }   }   /* averaging */   avr = (smax + smin) >> 1 ;   for ( i = 0 ; i < 8 ; i++ ) {   if ( *(cat+i) > avr ) { *(cat+i) = 1 ; }   else { *(cat+i) = 0 ; }   }   /* 80 bits => 8 bits */   result = 0 ;   for ( i = 0 ; i < 8 ; i++ ) {   result <<= 1 ;   result |= *(cat+i);   }   /* judge all one */   if ( res0 > 180 ) { result = MASKFF ; }   return result ;  }  黒と白の境目は、180と仮定しています。  実際のデータを見て、後でこの値を変更します。

タスク定義

 動作をすべて決めたので、タスクにまとめます。  ステートマシンを応用して、走行できるようにしています。  NORMAL、CRANK、LANE_CHANGEの3モードに分けてあります。  変数stateを利用して、もっと短くできますが、あまり短くすると  動作をトレースするのがしんどくなるので、3モードにしています。  3モードの中は、それぞれ機械的にステートを記述し、条件を入れています。  void tsk3_proc(void)  {   UBYTE i ;   UBYTE idx ;   UBYTE long_range ;   UBYTE short_range ;   /* get LongRange line */   get_line(SENTRY+XLAST*5,src0);   /* convert sensor data */   long_range = get_road(src0) & 0x81 ;   /* get ShortRange line */   get_line(SENTRY+XLAST*40,src1);   /* convert binary data */   short_range = get_road(src1);   /* judge */   if ( m_state == LANE_CHANGE ) {   /* sequencer */   switch ( state ) {   case 20 : /* straight */ state = 20 ; if ( long_range == 0x80 ) { state = 21 ; } if ( long_range == 0x01 ) { state = 25 ; } break ;   case 21 : /* turn right 45 degree */ state = 22 ; break ;   case 22 : /* move straight until detecting center white */ state = 22 ; if ( short_range == 0x18 ) { state = 23 ; } break ;   case 23 : /* turn left 45 degree */ state = 24 ; break ;   case 24 : /* return NORMAL mode */ m_state = NORMAL ; state = 0 ; break ;   case 25 : /* turn left 45 degree */ state = 26 ; break ;   case 26 : /* move straight until detecting center white */ state = 26 ; if ( short_range == 0x18 ) { state = 27 ; } break ;   case 27 : /* turn right 45 degree */ state = 28 ; break ;   case 28 : /* return NORMAL mode */ m_state = NORMAL ; state = 0 ; break ;   default : break ;   }   m_dir = state % 10 ;   /* straight */   if ( (m_dir % 2) == 0 ) { idx = LANE_CENTER ; }   /* turn right 45 degree */   if ( m_dir == 1 || m_dir == 7 ) { idx = LANE_RIGHT ; }   /* turn left 45 degree */   if ( m_dir == 3 || m_dir == 5 ) { idx = LANE_LEFT ; }   }   else   /* CRANK */   {   if ( m_state == LANE_CHANGE ) {   /* sequencer */   switch ( state ) {   case 10 : /* straight */ state = 10 ; if ( long_range == 0x80 ) { state = 11 ; } if ( long_range == 0x01 ) { state = 15 ; } break ;   case 11 : /* turn right 45 degree */ state = 12 ; break ;   case 12 : /* move straight until detecting center white */ state = 12 ; if ( short_range == 0x18 ) { state = 13 ; } break ;   case 13 : /* turn right 45 degree */ state = 14 ; break ;   case 14 : /* return NORMAL mode */ m_state = NORMAL ; state = 0 ; break ;   case 15 : /* turn left 45 degree */ state = 16 ; break ;   case 16 : /* move straight until detecting center white */ state = 16 ; if ( short_range == 0x18 ) { state = 17 ; } break ;   case 17 : /* turn left 45 degree */ state = 18 ; break ;   case 18 : /* return NORMAL mode */ m_state = NORMAL ; state = 0 ; break ;   default : break ;   }   m_dir = state % 10 ;   /* straight */   if ( (m_dir % 2) == 0 ) { idx = CRANK_CENTER ; }   /* turn right 45 degree */   if ( m_dir == 1 || m_dir == 5 ) { idx = CRANK_RIGHT ; }   /* turn left 45 degree */   if ( m_dir == 3 || m_dir == 7 ) { idx = CRANK_LEFT ; }   }   /* NORMAL */   else   {   /* judge directions */   if ( m_state == NORMAL ) {   /* default */   state = 0 ;   /* center */   if ( short_range == 0x18 ||   short_range == 0x1c ||   short_range == 0x38 ) { idx = NORMAL_CENTER ; }   /* move bit right */   if ( short_range == 0x30 ) { idx = NORMAL_BIT_RIGHT ; }   /* move right */   if ( short_range == 0x60 ) { idx = NORMAL_RIGHT ; }   /* move much right */   if ( short_range == 0xc0 ) { idx = NORMAL_MUCH_RIGHT ; }   /* move bit left */   if ( short_range == 0x0c ) { idx = NORMAL_BIT_LEFT ; }   /* left */   if ( short_range == 0x06 ) { idx = NORMAL_LEFT ; }   /* much left */   if ( short_range == 0x03 ) { idx = NORMAL_MUCH_LEFT ; }   /* all white */   if ( short_range == 0xff ||   short_range == 0x7f ||   short_range == 0x7e ||   short_range == 0xfe ) {   m_state = CRANK ;   state = 10 ; idx = CRANK_CENTER ;   }   /* left white */   if ( short_range == 0xf0 ||   short_range == 0xf8 ||   short_range == 0xe0 ||   short_range == 0xe8 ) {   m_state = LANE_CHANGE ;   state = 20 ; idx = LANE_CENTER ;   }   /* right white */   if ( short_range == 0x0f ||   short_range == 0x1f ||   short_range == 0x0e ||   short_range == 0x1e ) {   m_state = LANE_CHANGE ;   state = 20 ; idx = LANE_CENTER ;   }   }   }   }   /* set parameters */ left_duty = drive[idx].left ;   right_duty = drive[idx].right ;   dcount = drive[idx].wtime ;  }
目次

inserted by FC2 system