Micro Mouse
32×32区画用迷路探索シミュレータ(Win用655KB)
mm-nanoに搭載されている迷路探索ルーチンをWindows上でシミュレートするものです。
32×32区画用迷路探索シミュレータVer2.04(Win用677KB)
表示を1.5倍にして、設定値を保存できるようにして、ゴール位置2,3,4を省略できるようにしました。（2010/8/20）
32×32区画用迷路探索シミュレータのソースファイルです。
迷路探索シミュレータは著作権を放棄しています。どなた様もご自由に使用していただいて構いません。使用される時に私への連絡は必要ありません。
2009年のロボット学会誌(第27巻No.9)に掲載された解説です。 mm-nano (2007年製作)
.
.
.
.
.
マイクロマウス３ (2002年製作)
前方にある３組の赤外線反射式の光センサで迷路の壁を検出します。後方にも同じセンサがあります。というか前後の区別は無く、双方向に同じように走行できます。
６輪です。中の２輪が動輪、前後の４輪がステアリング輪です。ステアリング輪は前後２輪が一組になっており、反対向きに同期して動きます。中央に走行距離センサが見えます。青い２個のパッケージがバッテリです。
モータドライバ等も搭載したＣＰＵボードです。ＪＳＤのMC_SH7144を使用しています。
ＣＰＵボードを取った状態です。たすき掛けの状態がわかります。
奥からポテンショメータ、モータ、テンションプーリと並んでいます。
走行
マイクロマウス３はこんなふうに走ります(動画740KB)
スペック
全長
130mm
全高
52mm
全幅
60mm
重量
350g
タイヤ
外径φ25.5mm 幅10mm 京商MZW17-20(ゴム部) ×6
走行モータ
DCモータ 6V 2.25W 減速比70:20 ミニモータ社1724T006S ×2
走行用モータギヤ
協育歯車 S30B 20B+0302
走行用タイヤギヤ
協育歯車 S30B 70B+0203
ステアリングモータ
DCギヤモータ 6V 1.64W 減速比41:1 ミニモータ社1717P006S(16AK41:1) ×2
ステア用タイミングプーリ
ゲイツ･ユニッタ･アジア(旧ユニッタ) P24-1.5GT-60-33
ステア用タイミングベルト
ゲイツ･ユニッタ･アジア(旧ユニッタ) 252-1.5GT 幅3mm
ボールベアリング
NMB LF-520ZZ ×22個
各軸材
ミスミ 小径ロッドMRS2-100.0
ステアリング角度センサ
ポテンションメータ 日本サーボ VP12(5KΩ) ×2
壁センサ
赤外線反射センサ(リニア) ×6 (前方×1 左右斜35度×2 をマウス前後に2セット)
計測範囲
20mm～250mm
計測分解能
1mm程度
投光素子
オプトランス社 LS111P(投光半値角±1.8°)
受光素子
東芝 TPS708
走行距離センサ
ロータリエンコーダ 125パルス/回転 2相 アルファ技研MRE-012-125B ×1
計測分解能
0.088mm
方向センサ
圧電振動ジャイロ 村田製作所ENC-03J ×1
モータ駆動
FETのHブリッジをPWM駆動 FET素子 東芝2SK2231 PWM周波数 20KHz
電源
Ni-Cd 3.6V 300mA/h ×2 (φ15mm×30mmの3本組×2)
CPU
ルネサステクノロジ SH2/7144 動作クロック40MHz
ROM
256Kバイト(SH2内蔵フラッシュ)
RAM
512Kバイト
シリアルEEPROM
32Kバイト
開発ツール
GCC JDE
加速度(実測)
3.2m/sec^2
最高速度(実測)
2.6m/sec
旋回速度(実測)
0.7m～1.0m/sec
メカニズム
マイクロマウス３は６輪方式で
リアクションキックアップ というシステムを採用しています。
リアクションキックアップについては
Nさんのサイト の
日記 で詳しく解説されていますのでご覧ください。
迷路探索
足立法って何？
迷路探索アルゴリズムは足立法 を搭載しています。現在は医師である足立芳彦氏が福山マイコンクラブ時代（1984年、当時高校生）に発表したものです。~
マイクロマウス３では足立法を基本とした４つの探索方法を搭載しています。
未探索の区画が無くなるまでスタートとゴールの間を往復します。基本的な足立法です。
スタートからゴールまでの最短経路上の未探索区画に向かって探索します。
スタートとゴールの間を往復しますが現在位置から目的地までが通過済なら目的地を変更します。
最初のゴール後スタートからゴールまでの最短経路上の未探査区画に向かって探索します。
直線が好き
マウスは直線の方が速く走れます。ですから良い記録を出すためにはターンの多い経路より直線の多い経路を選ぶことが必要です。
迷路探索シミュレータ(Win用660KB)
マイクロマウス３に搭載されている迷路探索ルーチンをWindows上でシミュレートするものです。
迷路探索シミュレータのソース
C++Builder Ver5用 (27KB)
C++Builder Ver4用 (25KB)
迷路データ集(30KB)
迷路探索シミュレータ用の迷路データ集です。過去の大会の迷路データもあります。~
ちなみに迷路データは独自のバイナリ形式のほか、
Mさん ？のテキスト形式（下記例）も扱えます。
0x00000000,0x00000000,
0x00000000,0x00000000,
0x95555555,0x55555553,
0x853d1515,0x15555112,
0xc143c3c3,0xa9553eee,
0x943c3c3c,0x6a9547d3,
0xc16947c3,0x96ab9392,
0xd07c53bc,0x69686c6a,
0xd07d142b,0xd2d07956,
0xd0794168,0x387ad293,
0xd07c16b8,0x46907c6a,
0xd07943c0,0x156e9392,
0x9414147e,0xe93d2c6e,
0xc1414793,0x92ad4517,
0x9414796c,0x2ac553c3,
0xc1439439,0x429392ba,
0x90168128,0x3828282a,
0xeec56ec6,0xc6c6c6c6,
コーナリング
これは何でしょう？
これはマイクロマウス３が高速走行で180度ターンをしている時の状態をブラックボックス機能で収集したデータです。
ブラックボックス機能とはマウスの内部情報を定時間毎にメモリに保存しておく機能です。そのデータをパソコンに取り込み解析することによってマウスの中の見えなかった状態が見えるようになります。マイクロマウス３はこの機能のおかげで飛躍的に性能が上がりました。ブラックボックス機能を教えていただいた
Mさん に深謝。
ブラックボックスデータは左が新しく右が古いデータです。各データは下記のとおりで、1msec毎に収集しています。
[Ｓｐｅｅｄ] = 走行距離センサであるロータリエンコーダで計測した走行速度
コーナリングは３つの部分で構成されています。
１．進入部
２．定旋回部
３．直線へ向けての立ち上がり
マイクロマウス３は180度ターンの一連の動作を0.4秒程度で行います。目視やビデオ撮影ではとてもデバグできませんね。ブラックボックス機能様様です。
このように方向センサと走行距離センサでコーナリングを制御していますが、問題点があります。それは車体の方向と進行方向は一致しない という事実です。コーナリング中のタイヤは必ず横方向にスリップしています。そのスリップを検出しなければ完全な制御はできません。最近他の方々のマウスに搭載され始めている加速度センサは横方向のスリップ検出に有効なセンサかも知れません。
斜め走行
斜め走行はどう走る？
マイクロマウス３はジャイロの角速度を積分して方向を得ています。斜め走行ではジャイロで得た方向を元に走行します。
とりあえずぶつかりそうな壁は大きく避ける と目標とする方向は小さく補正する です。
DCモータの制御
モータ駆動方式
DCモータの駆動回路はFETのHブリッジをPWMで駆動しています。FET素子は東芝の2SK2231です。ハイサイドのFETを駆動するためのベース電圧は、トランジスタとコイルとダイオードで昇圧回路を作り供給しています。PWM周波数は20KHzでON-OFFの不連続電流モードです。PWM分解能は2000です。
ステアリングの制御
ステアリングはポテンションメータで位置を検出しPD制御で位置決めを行っています。~
DCモータの位置決め制御はとても簡単です。実際のＣ言語のプログラムで見てみましょう。
使用する変数はこれだけです。
int str_pos; // ステアリング指示値(-512～+511)
int str_real; // A/Dコンバータで読み込んだポテンションメータの値(-512～+511)
int str_spd; // ポテンションメータの値を微分して得たステアリング速度
int str_old; // ポテンションメータの値を微分するための1msec前の値
int str_pwm; // ステアリングモータに出力するPWM値(-2000～+2000)
そして1msec毎のタイマ割り込みで次のルーチンを実行します。
[ここでstr_realにポテンションメータの値を読み込み]
str_spd = str_real - str_old; // 前回の値との差をとってステアリング速度を得る
str_old = str_real; // 次回のために現在の値をメモしておく
str_pwm = (str_pos - str_real) * 25 - str_spd * 400; // PD制御部、25がPゲイン、400がDゲイン
if( str_pwm < -2000 ) str_pwm = -2000; // PWM値がオーバーフローしないようにリミッタを掛ける
if( str_pwm > 2000 ) str_pwm = 2000;
[ここでstr_pwmの値をモータに出力]
たったこれだけです。
ロータリエンコーダを読む
走行モータの制御のためには走行距離センサであるロータリエンコーダからデータを得なければなりません。ロータリエンコーダから走行距離（マウス位置）と走行速度を得ます。こちらもＣ言語のプログラムで見てみましょう。
使用する変数はこれです。
int dist; // 走行距離（マウス位置）
int speed; // 走行速度
short enc; // エンコーダを読んだ値(16bit)
short enc_old; // 前回のエンコーダ値
int spd0; // 前回のエンコーダ値と今回のエンコーダ値の差
int spd_old[9]; // 速度平均化のための配列
int t1,t2; // 作業用変数
そしてこれも1msec毎のタイマ割り込みで次のルーチンを実行します。
enc = (short)TCNT_1; // 16bitのアップダウンカウンタ値を読む
spd0 = (int)(enc-enc_old); // 前回の値との差を得る
enc_old = enc; // 次回のためにカウンタ値を保存
if( spd0 > 32768 ) spd0 -= 65536; // 16bitカウンタのオーバフロー時、アンダーフロー時を考慮する
if( spd0 < -32768 ) spd0 += 65536;
dist += spd0; // 32bit値の距離変数を積算
次に走行速度を求めます。TGRA_0はSH2/7144の機能であるインプットキャプチャ機能でロータリエンコーダのパルス幅を625KHzのクロックでカウントした値が入っています。TGRC_0はバッファ動作でTGRA_0の前回の値が入っています。正確な速度を求めるためにはエンコーダの片方の相のダウンエッジから次のダウンエッジ（あるいはアップエッジからアップエッジ）の間隔を計測する必要があるのですがSH2/7144にはその機能がありません。２相パルスのそれぞれの間隔を計測してしまいます。そこで平均化して速度を求めています。また、パルス間隔から計測する速度には符号がありませんから、位置や前回の速度から符号を付加します。
t1 = 53000/((int)TGRA_0+(int)TGRC_0); // パルス間隔の逆数を取って速度を得る(なぜ53000にしたのかは失念)
// 平均化する
t2 = (spd_old[8]+spd_old[7]+spd_old[6]+spd_old[5]+spd_old[4]+spd_old[3]+spd_old[2]+spd_old[1]+spd_old[0]+t1)/10;
spd_old[8] = spd_old[7];
spd_old[7] = spd_old[6];
spd_old[6] = spd_old[5];
spd_old[5] = spd_old[4];
spd_old[4] = spd_old[3];
spd_old[3] = spd_old[2];
spd_old[2] = spd_old[1];
spd_old[1] = spd_old[0];
spd_old[0] = t1;
if( spd0<0 ) { // 今回の位置の変化で符号を決定する
speed = -t2;
} else if( spd0>0 ) {
speed = t2;
} else { // 位置の変化が無いなら前回の速度の符号を引き継ぐ
if( speed>0 ) speed = t2;
else if( speed<0 ) speed = -t2;
else speed = 0;
}
これで位置(dist)と速度(speed)が得られました。
走行モータの制御
位置と速度が求まったのでこれを元に走行モータを制御します。
Ｃ言語のプログラムで見てみましょう。
使用する変数はこれです。
int mot_spd; // 走行モータの目標速度指示値
int mot_real; // 自動的に目標速度指示値まで加減速する変数
int up_dw; // 走行モータの加減速設定値(1～)
int up_rt; // 加減速を10msecに１回実行するためのタイマ
int pos_svo; // 速度制御するか位置決め制御するかのフラグ(1 or 0)
int lock_pos; // 位置決め制御する時の目標位置
int trq; // トルク制限値(0～2000)
int drv_pwm; // 出力PWM値(-2000～2000)
int t; // 作業用変数
そして1msec毎のタイマ割り込みで次のルーチンを実行します。
// 走行モータの加減速
if( ++ud_rt>=10 ) { // 加減速は10msecに１回行う（up_dwの分解能を稼ぐため）
ud_rt = 0;
if( mot_real<mot_spd ) { // 目標速度が高いなら加速
mot_real += up_dw;
if( mot_real>mot_spd ) mot_real = mot_spd;
} else if( mot_real>mot_spd ) { // 目標速度が低いなら減速
mot_real -= up_dw;
if( mot_real<mot_spd ) mot_real = mot_spd;
}
}
これでｍｏｔ＿ｓｐｄに目標の速度を設定するとｍｏｔ＿ｒｅａｌが自動的に加減速します。
// 走行モータフィードバック制御
if( !pos_svo ) { // 速度制御（走行時の制御）
drv_pwm = (speed - mot_real) * 30; // 速度だけのフィードバック
} else { // 位置制御（停止時の制御）
t = (lock_pos - dist) * 5; // 位置（Ｐ）フィードバック、5はＰゲイン
if( t < -2000 ) t = -2000; // リミッタを掛ける
if( t > 2000 ) t = 2000;
drv_pwm = t - speed * 20; // 速度（Ｄ）フィードバック、20はＤゲイン
}
if( drv_pwm < -trq ) drv_pwm = -trq; // トルク制限する
if( drv_pwm > trq ) drv_pwm = trq;
[ここで走行モータにdrv_pwmを出力] ＊実際はステアリング角に応じて左右の動輪のトルク(PWM)配分を変えています。(12/3追記)
停止した時にはｌｏｃｋ＿ｐｏｓに停止位置を入れてｐｏｓ＿ｓｖｏを1(true)にします。停止位置は遠くても大丈夫です。実際マイクロマウス３では探索時に停止する30mmくらい前から位置決めモードに入れてメインルーチンは迷路の探索ルーチンへ飛んでいます。
簡単ですね。難しい式なんて出てきません。いいかげんな制御です。
ジャイロを使う
方向センサに使用しているジャイロですが、これは出てくるデータを直接SH2/7144のA/Dコンバータで読んでいます。アンプとかフィルタは入れていません。リファレンス電圧も同じく直接読みます。そしてプログラム上でデータからリファレンスを引いた値を角速度とします。上記のコーナリングのブラックボックスデータでＧｙｒｏＳｐｄの値がこの角速度になります。
ジャイロといえばドリフト
ジャイロには出力電圧が温度等によってふらついてしまうドリフトがあります。角速度を積分して方向を求めようとすると小さなドリフトでも積算されてしまい無視できません。データシートにはドリフトの影響を無くすためにDC分をカットして使用する推奨回路が載っていますが、この回路は使用していません。
赤外線センサ
半値角±1.8度
下記はマイクロマウス３で使用している赤外線ＬＥＤのデータシートです。
OPTRANS Corp. IRLED LS111P データシート
1.概要
ＬＳ111Ｐは、高出力のＧａＡｌＡｓチップを使用した、890nm近赤外発光ダイオードです。
2.絶対最大定格（Ｔａ＝25℃）
項目
記号
定格値
単位
直流順電流
ＩＦ
110
ｍＡ
直流逆電圧
ＶＲ
5
Ｖ
パルス電流
ＩＦＰ
2 ※
Ａ
全電力損失
ＰＤ
175
ｍＷ
動作温度範囲
Topr
-30～+100
℃
保存温度範囲
Tstr
-30～+100
℃
ハンダ付温度
Tsol
260※※
℃
※ Ｔｗ＝10μS、Ｔ＝1mS~
3.電気的・光学的特性（Ｔａ＝25℃）
項目
記号
MIN.
TYP.
MAX.
単位
条件
発光出力
ＰＯ
3.5
ｍW
ＩＦ＝50ｍＡ
順電圧
ＶＰ
1.35
1.6
V
ＩＦ＝50ｍＡ
逆電流
ＩＲ
10
μＡ
ＶＲ＝5Ｖ
ピーク発光波長
λｐ
890
ｎｍ
ＩＦ＝50ｍＡ
発光波長半値幅
Δλ
80
ｎｍ
ＩＦ＝50ｍＡ
スイッチング速度（10～90％）
Ｔｒ
0.5
μＳ
ＩＦＰ＝50ｍＡ
Ｔｆ
0.5
μＳ
ＩＦＰ＝50ｍＡ
指向半値角
θ
±1.8
ｄeg.
NORIKO-93
1993年製作、2002年ライントレーサに転用のため分解。
.
前後にそれぞれ3組の赤外線センサを装備。反射強度によって壁までの距離を測定する。
.
4隅がステアリング輪、中2輪が動輪、中央の1個が距離計測輪。［1A］、［1B］と書いてあるのがバッテリ。
.
ステアリングは前後2個が逆位相に同期して動く。バッテリはコネクタによるワンタッチ着脱。 FETブリッジによるモータドライバ4CH、赤外線センサインタフェース、ロータリエンコーダインタフェース等が載った基板。設計が古いだけあってすべてDIP品。 CPUボード。NECのV55（12MHz）と日立のUPPを使用。プログラム、迷路データはEEPROMに格納。 ステアリング機構。タイミングベルトのたすきがけによって前後の逆位相を実現。アイドラープーリでモータ軸の歯跳びを抑制。 左に垂直に立っているのが村田製作所製の振動ジャイロ。
.
width
73mm
length
160mm
hight
75mm
weight
730g
tread(out side)
73mm
wheelbase
128mm
drive motor
DC motor *2 12V 3.33W gear retio 4.8:1 MINIMOTOR 2230U012S
steering motor
DC geared motor *2 12V 1.5W gear retio 41:1 MINIMOTOR 1624E012S(16/3Ktype 41:1)
steering position sensor
VR 5Kohm *2 JAPAN SERVO VP12
trip sensor
rotary encoder 125P/rev 2phase SEIKO EPSON MRE-012-125B
direction sensor
piezoelectric vibratory gyroscope MURATA GYRO STAR ENC-05D
wall sensor
infrared reflection sensor(lenear) *6
power source
Ni-Cd battery 4.8V 400mA/h *2
CPU
NEC V55(uPD70433) clock 12MHz
controle LSI
HITACHI UPP(HD63140) Universal Pulse Processor
ROM
EEPROM 128Kbyte
RAM
128Kbyte
program language
TURBO-C++
tool
EPSON PC-386NAR
NORIKO-93と開発当初のマイクロマウス3（2002年9月23日撮影）
.
NORIKO-89
1989年製作。ひし形配置の4輪。走行モータはステッパ、ステアリングモータはDC。
.
フライホイールをロータリエンコーダにぶら下げてジャイロ代わりの方向センサに。 墓所
-
最終更新：2021年04月17日 05:17
Maze32.zip
