Mouse32×32区画用迷路探索シミュレータ(Win用655KB)Maze32.zipmm-nanoに搭載されている迷路探索ルーチンをWindows上でシミュレートするものです。色々な迷路を作り、探索させることが出来ます。迷路の区画数を設定できるVer2.03をアップしました。(2010/8/20)32×32区画用迷路探索シミュレータVer2.04(Win用677KB)Maze32_2.zip表示を1.5倍にして、設定値を保存できるようにして、ゴール位置2,3,4を省略できるようにしました。(2010/8/20)32×32区画用迷路探索シミュレータのソースファイルです。Maze32source.zip迷路探索シミュレータは著作権を放棄しています。どなた様もご自由に使用していただいて構いません。使用される時に私への連絡は必要ありません。2009年のロボット学会誌(第27巻No.9)に掲載された解説です。(2007年製作)ハーフサイズマイクロマウスのmm-nanoです。.....組み立て前(基板は古いバージョンです)。(2002年製作)前方にある3組の赤外線反射式の光センサで迷路の壁を検出します。後方にも同じセンサがあります。いうか前後の区別は無く、双方向に同じように走行できます。中の2輪が動輪、前後の4輪がステアリング輪です。ステアリング輪は前後2輪が一組になっており、反対向きに同期して動きます。中央に走行距離センサが見えます。青い2個のパッケージがバッテリです。モータドライバ等も搭載したCPUボードです。JSDのMC_SH7144を使用しています。タイミングベルトで前後のステアリングとモータ、ポテンションメータが繋がっています。ベルトを「たすき掛け」にすることにより、前後のステアリングを逆位相で同期させています。CPUボードを取った状態です。たすき掛けの状態がわかります。奥からポテンショメータ、モータ、テンションプーリと並んでいます。0.7m~1.0m/secメカニズムマイクロマウス3は6輪方式でリアクションキックアップというシステムを採用しています。何?迷路探索アルゴリズムは足立法を搭載しています。現在は医師である足立芳彦氏が福山マイコンクラブ時代(1984年、当時高校生)に発表したものです。~当時、一般の迷路探索法はまずマウスの居る位置の壁の情報やマウスの居る絶対位置からを進む方向を決定し、壁の情報を全て収集した後に最短経路を計算するという方法がほとんどでした。足立法ではまず最初に最短経路を求め、その最短経路に沿って走行しながら壁の情報を集めます。この時未探索の壁は無いものとします。求めた最短経路通りに進めなくなると既知の壁を考慮して最短経路を求め直します。この方法でスタートとゴールを往復し、最短経路に未探索の区画が無くなると探索終了で最短経路が求まっています。マイクロマウス3では足立法を基本とした4つの探索方法を搭載しています。アルゴリズム0未探索の区画が無くなるまでスタートとゴールの間を往復します。この方法の欠点は毎回スタート位置に戻ってしまうことです。スタート位置に戻ってしまうとルール上、多数回のトライとみなされてしまいます。アルゴリズム1スタートからゴールまでの最短経路上の未探索区画に向かって探索します。スタートからゴールまでの最短経路計算と、マウスの居る位置から未探索区間への最短経路計算の2回の計算が必要なので時間がかかります。スタートからゴールまでの最短経路は一区画探索して壁が確定するたびに変わることもありますから、あっちやこっちを一区画ずつ探しに行く現象(発振状態)になる場合があります。アルゴリズム2スタートとゴールの間を往復しますが現在位置から目的地までが通過済なら目的地を変更します。アルゴリズム0でスタートへ戻ってしまう欠点を補った方法です。最短経路計算も1回で済みます。マイクロマウス3は現在ではこのアルゴリズムに固定しています。~アルゴリズム3最初のゴール後スタートからゴールまでの最短経路上の未探査区画に向かって探索します。最初のゴールまではアルゴリズム0でその後アルゴリズム1に切り替えます。直線が好きマウスは直線の方が速く走れます。良い記録を出すためにはターンの多い経路より直線の多い経路を選ぶことが必要です。最短経路を求める時に直線は1、ターンは2、あるいはそれ以上と、各区画の重み付けを変えることによって直線が多い経路を選択させることができます。階段状の経路は斜め走行で走ると直線とみなせますから、それも優先させなければなりません。マイクロマウス3では直前のターンの反対のターンの重みを一番軽くすることによって斜めを優先させています。おバカなようで、昨年(2002年)の決勝迷路では下記のような経路を選択してしまいました。迷路探索シミュレータ(Win用660KB)Maze.exeマイクロマウス3に搭載されている迷路探索ルーチンをWindows上でシミュレートするものです。色々な迷路を作り、探索させることが出来ます。(25KB)Maze4.zip迷路データ集(30KB)MAZ.zip迷路探索シミュレータ用の迷路データ集です。過去の大会の迷路データもあります。~ちなみに迷路データは独自のバイナリ形式のほか、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,コーナリングこれは何でしょう?これはマイクロマウス3が高速走行で180度ターンをしている時の状態をブラックボックス機能で収集したデータです。ブラックボックス機能とはマウスの内部情報を定時間毎にメモリに保存しておく機能です。そのデータをパソコンに取り込み解析することによってマウスの中の見えなかった状態が見えるようになります。マイクロマウス3はこの機能のおかげで飛躍的に性能が上がりました。ブラックボックス機能を教えていただいたMさんに深謝。ブラックボックスデータは左が新しく右が古いデータです。各データは下記のとおりで、1msec毎に収集しています。ステアリング角度センサであるポテンションメータで計測した実ステアリング角度(符号が反対になっています)コーナリングは3つの部分で構成されています。1.進入部まずマウスの最終方向が0となるように方向(Gyro)をプリセットします。ステアリングを走行距離に比例して切って行きます。ステアリング指示値(StrSet)が直線的に増加し、やや遅れて実ステアリング角(StrR)が追従しています。遅れて角速度(GyroSpd)が発生しています。2.定旋回部ステアリングを一定角度まで切ったら、定旋回に入ります。この時、旋回半径を一定にするために走行速度(Speed)と角速度(GyroSpd)の比が一定になるようにステアリングを制御しています。その影響で定回転部の最初に部分でステアリング指示値(StrSet)が大きく乱れています。改良の余地ありですね。3.直線へ向けての立ち上がり直線へ向けてステアリングを戻す部分です。ここではマウスの方向(Gyro)をP、角速度(GyroSpd)をDとしたPD制御でステアリングを制御しています。Pゲイン、Dゲインを調整するとキビキビした立ち上がりにしたり、ゆったりとした立ち上がりにしたりと味付けができます。~定旋回部からPD制御部への切り替えですが、定旋回部でもPD制御の演算を行っておりその結果はグラフで示した赤い線のような値になります。定旋回部で得たステアリング指示値とPD制御の演算結果がクロスした時点で切り替えることによってステアリングの乱れを無くしています。マイクロマウス3は180度ターンの一連の動作を0.4秒程度で行います。目視やビデオ撮影ではとてもデバグできませんね。ブラックボックス機能様様です。このように方向センサと走行距離センサでコーナリングを制御していますが、問題点があります。それは車体の方向と進行方向は一致しないという事実です。コーナリング中のタイヤは必ず横方向にスリップしています。そのスリップを検出しなければ完全な制御はできません。最近他の方々のマウスに搭載され始めている加速度センサは横方向のスリップ検出に有効なセンサかも知れません。斜め走行はどう走る?マイクロマウス3はジャイロの角速度を積分して方向を得ています。斜め走行ではジャイロで得た方向を元に走行します。ジャイロは割りと正確な方向を出してくれるのですが斜め走行を補正無しで走れるほどの精度は出ません。そこで少ない壁情報を元に補正を掛けます。補正の考えはとりあえずぶつかりそうな壁は大きく避けると目標とする方向は小さく補正するです。左の図で破線が斜め走行に入った時の目標方向を示します。このままでは壁にぶつかってしまいます。そこで実線のようにとりあえずの壁は大きく避けて目標とする方向を少し修正するようにステアリングを操作します。右の図はマウスが光センサでみた壁の状態を示します。壁との距離が破線で示した数値を超えるとそのデータをそのままステアリングにフィードバックします(赤い線の部分)。これによりステアリングを大きく切って元に戻すという操作を実現しています。この操作をした時にジャイロで求めた方向を少し修正します。DCモータの制御モータ駆動方式DCモータの駆動回路はFETのHブリッジをPWMで駆動しています。FET素子は東芝の2SK2231です。ハイサイドのFETを駆動するためのベース電圧は、トランジスタとコイルとダイオードで昇圧回路を作り供給しています。PWM周波数は20KHzでON-OFFの不連続電流モードです。PWM分解能は2000です。ステアリングの制御ステアリングはポテンションメータで位置を検出しPD制御で位置決めを行っています。~ラジコンのサーボモータと似たようなものですが、ラジコンサーボはステアリングに使用できません。求められる動作速度が足りないからです。マイクロマウス3の性能を満たすためにはラジコンサーボの2倍以上の動作速度が必要です。DCモータの位置決め制御はとても簡単です。実際のC言語のプログラムで見てみましょう。使用する変数はこれだけです。ステアリングモータに出力するPWM値(-2000~+2000)そして1msec毎のタイマ割り込みで次のルーチンを実行します。2000;[ここでstr_pwmの値をモータに出力]たったこれだけです。Pゲインの25、Dゲインの400は動作を見ながら調整します。ロータリエンコーダを読む走行モータの制御のためには走行距離センサであるロータリエンコーダからデータを得なければなりません。ロータリエンコーダから走行距離(マウス位置)と走行速度を得ます。こちらもC言語のプログラムで見てみましょう。使用する変数はこれです。作業用変数そしてこれも1msec毎のタイマ割り込みで次のルーチンを実行します。走行距離(マウス位置)を求めます。TCNT_1はSH2/7144の機能である2相パルスのアップダウンカウント機能(位相計数モード4)を使用したエンコーダカウンタです。16bitカウンタなのでオーバーフローを回避するために、前回との差をとってint値(32bit)に加算します。次に走行速度を求めます。TGRA_0はSH2/7144の機能であるインプットキャプチャ機能でロータリエンコーダのパルス幅を625KHzのクロックでカウントした値が入っています。TGRC_0はバッファ動作でTGRA_0の前回の値が入っています。正確な速度を求めるためにはエンコーダの片方の相のダウンエッジから次のダウンエッジ(あるいはアップエッジからアップエッジ)の間隔を計測する必要があるのですがSH2/7144にはその機能がありません。2相パルスのそれぞれの間隔を計測してしまいます。そこで平均化して速度を求めています。パルス間隔から計測する速度には符号がありませんから、位置や前回の速度から符号を付加します。0;}これで位置(dist)と速度(speed)が得られました。位置と速度が求まったのでこれを元に走行モータを制御します。マイクロマウス3は停止時はPD制御による位置決め制御、走行時は速度制御だけを行っています。C言語のプログラムで見てみましょう。使用する変数はこれです。作業用変数そして1msec毎のタイマ割り込みで次のルーチンを実行します。}}これでmot_spdに目標の速度を設定するとmot_realが自動的に加減速します。*実際はステアリング角に応じて左右の動輪のトルク(PWM)配分を変えています。(12/3追記)停止した時にはlock_posに停止位置を入れてpos_svoを1(true)にします。停止位置は遠くても大丈夫です。実際マイクロマウス3では探索時に停止する30mmくらい前から位置決めモードに入れてメインルーチンは迷路の探索ルーチンへ飛んでいます。難しい式なんて出てきません。いいかげんな制御です。DC2輪のマウスはこんな制御ではまともに動きません。6輪マウスはステアリングがあるので走行モータを正確に制御する必要がないのです。ステアリング制御も負荷がほとんどありませんから簡単な制御で正確な位置決めが可能です。モータがたくさん付いていて難しそうに見えて実は簡単なのです。ジャイロを使う方向センサに使用しているジャイロですが、これは出てくるデータを直接SH2/7144のA/Dコンバータで読んでいます。アンプとかフィルタは入れていません。リファレンス電圧も同じく直接読みます。プログラム上でデータからリファレンスを引いた値を角速度とします。上記のコーナリングのブラックボックスデータでGyroSpdの値がこの角速度になります。A/Dコンバータは0-3.3V、10bit分解能ですから1bit当たり約3.22mVです。100程度の数値が出ているということは実際の電圧は322mVで、ジャイロの感度は0.67mv/°/sですから480°/sで旋回しているということになります。ジャイロの検出範囲である±300°/sを超えていますがデータは実際には広範囲にわたりリニアに出ているようで、実用上問題はありません。前作のマイクロマウスNORIKO-93の時は、同じく村田製作所の旧シリーズであるENC-05Dを使用していたのですが、こちらは検出範囲が±90°/sであり、この範囲を超えるとリニアでなくなるので補正テーブルを使用していました。ジャイロといえばドリフトジャイロには出力電圧が温度等によってふらついてしまうドリフトがあります。角速度を積分して方向を求めようとすると小さなドリフトでも積算されてしまい無視できません。データシートにはドリフトの影響を無くすためにDC分をカットして使用する推奨回路が載っていますが、この回路は使用していません。マイクロマウス3ではドリフトの影響を極力少なくするために、走り出す前の停止した状態で60msec分ジャイロデータをサンプリングし、その平均値を基準として角速度を求めるようにしています。この処理は探索走行では止まるたびに、高速走行では走り出す前に行います。高速で直線を走行している時に両壁があるならジャイロの積分値をリセットしています。ターンや両壁がない所のみでジャイロに頼り、直線では壁でジャイロを補正してやるという考えかたです。下記はマイクロマウス3で使用している赤外線LEDのデータシートです。注目すべきは指向半値角±1.8度、パルス電流2A(10μS)でしょうか。他のLEDと比べてどれくらい良いかと言うと・・・・、っと私は他のLEDを使ったことがないのでわかりません。データシート1.概要LS111Pは、高出力のGaAlAsチップを使用した、890nm近赤外発光ダイオードです。10-18ステムに非球面レンズカンをハーメチックシールした高信頼性タイプです。非球面レンズ形状により従来のメタルカンタイプでは不可能だった狭指向性を実現しています。各種オプトスイッチ、センサー及びエンコーダー用光源に適しています。deg.NORIKO-931993年製作、2002年ライントレーサに転用のため分解。.前後にそれぞれ3組の赤外線センサを装備。反射強度によって壁までの距離を測定する。.4隅がステアリング輪、中2輪が動輪、中央の1個が距離計測輪。[1A]、[1B]と書いてあるのがバッテリ。.ステアリングは前後2個が逆位相に同期して動く。バッテリはコネクタによるワンタッチ着脱。FETブリッジによるモータドライバ4CH、赤外線センサインタフェース、ロータリエンコーダインタフェース等が載った基板。設計が古いだけあってすべてDIP品。NECのV55(12MHz)と日立のUPPを使用。プログラム、迷路データはEEPROMに格納。タイミングベルトのたすきがけによって前後の逆位相を実現。アイドラープーリでモータ軸の歯跳びを抑制。左に垂直に立っているのが村田製作所製の振動ジャイロ。.MさんのサイトでNORIKO-93の動画(1.3MB)が見られます。PC-386NARNORIKO-93と開発当初のマイクロマウス3(2002年9月23日撮影).NORIKO-891989年製作。走行モータはステッパ、ステアリングモータはDC。中央には距離計測用の従輪。.フライホイールをロータリエンコーダにぶら下げてジャイロ代わりの方向センサに。NさんのサイトにNORIKO-XX1の写真とNORIKO-91の写真があります。墓所NORIKO-XX1の現在の姿。NORIKO-89(改)の現在の姿。NORIKO-92の現在の姿。NORIKO-FINALの現在の姿。