Cartサブシステム仕様.

Presentation on theme: "Cartサブシステム仕様."— Presentation transcript:

1 Cartサブシステム仕様

2 責務 Pilot Navigator Cart （アクティブクラス）
　System Controlerから走行開始命令を受け、ゴールに到達したらSystem Controlerに到着を報告する。 　Nagivatorにゴールに到達するためのコマンド列を問い合わせ、コマンド列の実行をCartに指示する。 　Cartから報告されるコマンド列の実行結果と壁情報を使って、Navigatorに自車位置とマップの壁情報の更新を指示する。 Navigator 　迷路情報、自車位置を保持し、更新する。 　目的地点（ゴール／スタート）に到達したかどうかを判定する。 　目的地点（ゴール／スタート）に到達するまでの最短経路を導出する。 　現在の自車位置から目的地点（ゴール／スタート）に到達するためのコマンド列を生成する。 Cart （アクティブクラス） 　走行コマンド列を実行して、実行結果と移動したブロックごとの壁情報を報告する。 　走行コマンドの実行にあたり、姿勢制御や衝突回避を行う。

3 Navigator ドメインはパッシブにすることで、ＰＣ上でシミュレーションできるようにする。
探索走行時のコラボレーション System Controler isGoal() getNextCommand() Pilot Navigator 6. updateMap( responseQueue ) responseQueue 3. setCommand( commandQueue ) Navigator ドメインはパッシブにすることで、ＰＣ上でシミュレーションできるようにする。 Cart 4. getWall( ) Wall Sensor 5. setVelocity() Motor Cartサブシステム

4 走行コマンドの種類 ２００６年度版は こちらだけサポートする

5 走行コマンドとレスポンス レスポンスをブロック単位に分解してしまう 迷路の壁情報更新は、１ブロックずつ行いたい。
しかし、１ブロック移動しては止まるというのは移動が遅くなるからやりたくない。 例えば５ブロック前進して、レスポンスとして５ブロック分の壁情報が得られるのが好ましい。 レスポンスをブロック単位に分解してしまう 走行コマンドを最後まで実行 途中で壁を検知して停止 前進１、壁＝101 前進１、壁＝110 右９０、壁＝101 停止、壁＝101 正常終了 走行コマンド列（入力） 前進１、壁＝101 前進１、壁＝111 右９０、壁＝110 継続不可 前進　３ 右スピン９０ 停止 終了

6 コマンド実行制御フロー 速度区間設定 加速 一定速度 減速 停止 距離補正 レスポンス用に １ブロックごとの 壁情報を取っておく
移動距離から加減速時間、巡航時間を決める。 速度区間設定 加速テーブルに従い、所定時間で加速する。 加速距離は０．５ブロック。 ０．５ブロック移動するごとに前方の壁の有無をチェックする。０．５ブロック前方に壁を見つけたら減速に移る。 ０．５ブロック移動するごとに前方の壁の有無をチェックする。壁を見つけたら減速に移る。 加速 一定速度 所定時間だけ、一定速度で走行する。 減速 加速テーブルを逆にたどって、所定時間で減速する。 減速距離は０．５ブロック。 レスポンス用に １ブロックごとの 壁情報を取っておく 停止 前方に壁がある AD値と壁判定閾値を比較して、差が所定範囲内に収まるまでゆっくり一定速度で動く。 FALSE TRUE 距離補正 コマンド完了後は移動ブロックごとの壁情報をレスポンスとして返す。

7 Ｎブロックを走行するための加減速 速度 時間 Lacc = (Vhigh + Vlow) Tacc / 2
前壁チェック 速度 ０．５ブロックごとに 前壁チェック 上限速度 前壁チェック 加速距離 巡航距離 減速距離 下限速度 ０．５ブロック Ｎ－１ブロック ０．５ブロック 時間 加減速時間 巡航時間 加減速時間 調整時間 台形の面積を求める公式 上限速度 Vhigh 下限速度 Vlow 加速時間 Tacc 巡航時間 Tc 加速距離 Lacc 巡航距離 Lc 加速時間を求める 巡航時間を求める Lacc = (Vhigh + Vlow) Tacc / 2 ∴ Tacc = 2 Lacc / (Vhigh + Vlow) Lc = Vhigh Tc ∴ Tc = Lc / Vhigh

8 Lacc = (Vhigh + Vlow) Tacc / 2 ∴ Tacc = 2 Lacc / (Vhigh + Vlow)
加速時間を求める 巡航時間を求める Lacc = (Vhigh + Vlow) Tacc / 2 ∴ Tacc = 2 Lacc / (Vhigh + Vlow) Lc = Vhigh Tc ∴ Tc = Lc / Vhigh 例１） Lacc = 90 [mm] Vhigh = 300 [mm/s] Vlow = 50 [mm/s] の場合 Tacc = 2 * 90 / ( ) = [sec] 例２） Vhigh = 600 [mm/s] Tacc = 2 * 90 / ( ) = [sec] 例１）　２区画前進, Vhigh = 300 [mm/s] Lc = 180×2 － 90×2 = 180 [mm] Tc = 180 / 300 = 0.6 [sec] 例２）　６区画前進, Vhigh = 300 [mm/s] Lc = 180×6 － 90×2 = 900 [mm] Tc = 900 / 300 = 3 [sec]

9 加速テーブル 制御周期当たりの速度増分を求める式 Δv = (Vhigh – Vlow) / Tacc ×Δt 上限速度 速度増分 加速距離
例） Δｔ = 0.02 ［s］ Lacc = 90 [mm] Vhigh = 300 [mm/s] Vlow = 50 [mm/s] の場合 Tacc = 0.51 [ms] Δv = (300 – 50) / 0.51×0.02 = 9.8 [mm / s] 上限速度 速度増分 加速距離 下限速度 加減速時間 制御周期 制御周期当たりの速度増分を求める式 Δv = (Vhigh – Vlow) / Tacc ×Δt

10 スピンの加減速 速度 時間 Lr = Ll = θ・T/2 トレッド T 右車輪の移動距離 Lr 左車輪の移動距離 Ll スピン角度 θ
上限速度 加速距離 巡航距離 減速距離 下限速度 45° ０～９０° 45° 時間 加減速時間 巡航時間 加減速時間 調整時間 車輪の回転方向を逆にすれば、後は距離の管理で角度を管理できる。 Lr = Ll = θ・T/2 加減速時間、巡航時間はそれぞれ 直進時と同様に求めることができる。 トレッド 　　　　　　T 右車輪の移動距離 Lr 左車輪の移動距離 Ll スピン角度　　　　　θ T = 74 [mm], θ=45°のとき Lr = Ll = 45π/180×74/2 = [mm] θ=45° のとき [mm] θ= 90°のとき [mm] θ=180°のとき [mm]

11 Lacc = (Vhigh + Vlow) Tacc / 2 ∴ Tacc = 2 Lacc / (Vhigh + Vlow)
加速時間を求める 巡航時間を求める Lacc = (Vhigh + Vlow) Tacc / 2 ∴ Tacc = 2 Lacc / (Vhigh + Vlow) Lc = Vhigh Tc ∴ Tc = Lc / Vhigh 例１） Lacc = [mm] Vhigh = 140 [mm/s] Vlow = 50 [mm/s] の場合 Tacc = 2 * / ( ) = [sec] 例１）　180°スピン, Vhigh = 140 [mm/s] Lc = － 29.06×2 = [mm] Tc = / 140 = [sec] 45°加速 90°巡航 45°減速 で合計 1.00[sec] 制御周期当たりの速度増分 Δv = (Vhigh – Vlow) / Tacc ×Δt Δv = (140 – 50)/0.306×0.02 = 5.88 [mm/s] θ=45° のとき [mm] θ= 90°のとき [mm] θ=180°のとき [mm]

12 直進時の姿勢制御 左車輪を 加速する 左車輪を 減速する 左車輪を 通常速度に戻す 左車輪を 加速する 左車輪を 減速する 左車輪を
左に壁がある FALSE 右に壁がある FALSE TRUE 車体が左寄り FALSE 車体が左寄り FALSE TRUE 車体が 右寄り TRUE 車体が 右寄り FALSE FALSE TRUE TRUE 左車輪を 加速する 左車輪を 減速する 左車輪を 通常速度に戻す 左車輪を 加速する 左車輪を 減速する 左車輪を 通常速度に戻す 左車輪を 通常速度に戻す

13 Cartクラス Cart CommandQueue AccTable Motor 1 setCommand（command）
仕組みはリングバッファでＯＫ 1 setCommand（command） 3 execCommand（command） 2 put( command ) 4 get( &command) Cart CommandQueue コマンドキューのサイズはいくつあったら足りるだろうか？ AccTable Motor 第２１回の迷路で数えると、スタートからゴールまで最短で６０コマンドくらい必要。 120コマンドまで入るようにすると １コマンド1Byteで作って、120Byte必要。