目次
前
次
走行パターン生成
MCRのコースは、直線、曲線、クランク、レーンチェンジで
構成されています。
画像データよりコースを把握して、左右のモータに与える
Duty比を決め、内蔵モジュールに設定すると、マシンは動
きます。
今回は、コースの状態を次の3つに分類しました。
- NORMAL 直線、曲線走行
- CRANK 90度クランク走行
- LANE_CHANGE レーンチェンジ処理
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比を決めます。
- 中央に白がある 左右のDuty比を同一に設定
- 中央から右に白がある 左Duty>右Dutyに設定
- 中央から左に白がある 左Duty<右Dutyに設定
- すべて白である 左右のDuty比を同一に設定(NORMAL→CRANK)
- 中央から右がすべて白 左右のDuty比を同一に設定(NORMAL→LANE_CHANGE)
- 中央から左がすべて白 左右のDuty比を同一に設定(NORMAL→LANE_CHANGE)
曲線走行では、白が中央から左右にどれだけ離れているかを
判断し、Duty比の値を増減させます。
NORMAL→CRANKあるいはNORMAL→LANE_CHANGEでは、徐行スピード
で直進します。
クランク走行
クランクでは、5つの状態に分けて走行します。
- LongRangeで、左か右の端に白がなくなるまで直進
- 左か右の端に白がない側に45°回頭
- ShortRangeで、中央に白を検出するまで直進
- 45°回頭
- 指定時間後、CRANK→NORMALと状態遷移
左か右の端に白がない側に45°回頭するのは、確実に
中央の白を検出できるようにするためです。
45°回頭するために必要になる時間は、実際に走行させて
決めます。
レーンチェンジ走行
レーンチェンジ走行では、7つの状態に分けて走行します。
- LongRangeで、左か右の端に白がなくなるまで直進
- 左か右の端に白がない側に45°回頭
- 指定時間直進
- ShortRangeで、中央に白を検出するまで直進
- 45°回頭し、マシンを進行方向にあわせる
- 指定時間直進
- CRANK→NORMALと状態遷移
3、6で必要となる時間は、実際に走行させて決めます。
画像処理
画像処理は、80ピクセルを80ビットに変換する2値化とします。
最初に白と黒を判定するための閾値を求めておきます。
走行前に、4回画像を取得して、白と黒の平均を求めます。
4回とするのは、利用したSRAMが32kバイトで、80x60の画像
データを4枚まで保存できるからです。
上で求めた値を利用して、画像の2値化のための閾値を求めて
おき、次の処理を実行します。
1ピクセルは8ビットなので、0〜255までの数値をとります。
80ピクセルの最大値、最小値を求めて、相加平均を計算します。
相加平均を下回った値を0、それ以外を1にします。
相加平均が、先に求めた閾値より小さい場合は、閾値を利用して
0と1を決めます。
2値化のシーケンスは、以下とします。
- 0ピクセルのデータを、仮の最大、最小とする
- Mを1とする
- Mピクセルのデータが、最大より大きい場合、そのデータを最大とする
- Mピクセルのデータが、最小より小さい場合、そのデータを最小とする
- Mの値を1増やす
- M<80のとき、2に戻る
- 最大、最小の相加平均を求める
- Mを0とする
- Mピクセルのデータが、相加平均より小さい場合、0とする
- Mピクセルのデータが、相加平均以上の場合、1とする
- Mの値を1増やす
- 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 ;
}
目次
前
次